!==========================================================================
! BSAM: Block-Structured Adaptive Multigrid Solver
!==========================================================================
!
! (c) Copyright Steven M. Wise, 2006
! Department of Mathematics
! University of California at Irvine
! swise@math.uci.edu
!
! (c) Copyright Steven M. Wise, 2007
! Department of Mathematics
! University of Tennessee
! swise@math.utk.edu
!
! Portions of the code
!
! (c) Copyright Sorin Mitran, 2002
! Department of Mathematics
! University of North Carolina at Chapel Hill
! mitran@amath.unc.edu
!
! -----------------------------------------------------------------------
! This software is made available for research and instructional use only.
! You may copy and use this software without charge for these
! non-commercial purposes, provided that the copyright notice and
! associated text is reproduced on all copies.  For all other uses,
! including distribution of modified versions, please contact the authors.
!
! Commercial use is strictly forbidden without permission.
!
! This software is made available "as is" without any assurance that it
! will work for your purposes. The software may in fact have defects,
! so use the software at your own risk.
!
! -----------------------------------------------------------------------
! File:             problem.f90
! Purpose:          BSAM problem module.
! Contains:
! Revision History: Ver. 1.0 Oct. 2006 Steven Wise
! Revision History: Ver. 1.1 May  2007 Steven Wise
! -----------------------------------------------------------------------
module Problem
!
! This module contains routines for initializing the problem and for driving
! the multigrid routines in the module AFASRoutines.  In particular, the
! routines in AFASRoutines use RelaxGrid#D, Operator#D, Source#D,
! SourceUpdate#D, and UpdateAuxVcycle#D, where # = 2 and 3.
!
! Templates for the Diffuse Interface Tumor Model are included.
!
contains
!
subroutine SetProb
use NodeInfoDef
use ProblemDef
implicit none
!
integer:: ierror
NAMELIST/problemdata/axisym,eps,mu,rho,g,radius, b_eq &
                    ,beta,f_eq,f_inf,r_a,r_d,surfdiff,bulkdiff,cavity_speed,cap
!
OPEN(UNIT=75,FILE='problemdata.dat',STATUS='OLD',ACTION='READ',IOSTAT=ierror)
if(ierror/=0) then
  PRINT *,'Error opening input file problemdata.dat. Program stop.'
  STOP
end if
READ(75,NML=problemdata)
CLOSE(75)
OPEN(UNIT=76,FILE='output.dat',STATUS='OLD',ACTION='WRITE',FORM='FORMATTED', &
     POSITION='APPEND')
WRITE(76,NML=problemdata)
CLOSE(76)
!
eps2 = eps*eps

end subroutine SetProb
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
subroutine AfterRun
use NodeInfoDef
use ProblemDef
implicit none
!
end subroutine AfterRun
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
subroutine QInit2D(q,mx,nrvars,h,xlower)
use NodeInfoDef
use GridUtilities, only: ULap2D
use ProblemDef
implicit none
!
real, dimension(1:,1:,1:), intent(out):: q
integer, dimension(1:2), intent(in):: mx
integer, intent(in):: nrvars
real, intent(in):: h
real, dimension(1:2), intent(in):: xlower
real,dimension(2) :: xmax
!
integer:: i, j
real:: delta, x, y, z,rad,r
real :: pi,xc,yc,theta,ecc,a,b

xmax = xlower + mx*h
delta = eps*sqrt(2.0)
pi=4.0*atan(1.0)
! Circle parameters
xc = 0.0
yc = 0.0
! Ellipse parameters
! a = radius*1.05
! b = radius/1.05**2
! ecc = sqrt(a**2-b**2)/a
! sigma0 = mu(1) / cap / (1.0-beta*f_eq/f_inf)
sigma0 = mu(1) / cap / (1.0+beta*log(1.0-f_eq/f_inf))
print *, 'sigma0 = ', sigma0
do i = 1, mx(1)
  x = (real(i,kind=r8)-0.5)*h+xlower(1)
  do j = 1, mx(2)
    y = (real(j,kind=r8)-0.5)*h+xlower(2)

    r = sqrt((x-xc)**2+(y-yc)**2)
    ! Circle
    theta = atan2(x-xc,y-yc)
    q(i,j,qc) = 0.5*(1.0-tanh((r-radius)/(2.0*delta)))

    q(i,j,qf) = f_eq!1.0+0.2*cos(2.0*theta)
!     q(i,j,qf) = 1.0+0.2*cos(2.0*theta)
    q(i,j,qb) = b_eq !0.5*(1.0-0.2*cos(2.0*theta))

    ! Ellipse
!     theta = atan2(y-yc,x-xc)
!     rad = b/sqrt(1-(ecc*cos(theta))**2)
!     q(i,j,qc) = 0.5*(1.0-tanh((r-rad)/(2.0*delta)))
    ! ratz and voigt formulation
    ! this has 0.5 closer to where it should be..:P
    ! But how to modify the CH equation?
!     q(i,j,qc) = 0.5*(1.0-tanh(3.0*(r-rad)/eps))

    ! Perturbated line
!     rad = 0.5 - 0.05*cos(y)
!     q(i,j,qc) = 0.5*(1.0-tanh((x-rad)/(2.0*delta)))
!     rad = 0.5 - 0.01*cos(2*pi*x)
!     q(i,j,qc) = 0.5*(1.0-tanh((y-rad)/(2.0*delta)))

    q(i,j,qmu) = 0.0
    q(i,j,qp) = 0.0    
    q(i,j,qpsi) = 0.0    
!    q(i,j,qsig)= sigma0*(1.0 - beta*q(i,j,qf))
    q(i,j,qsig)= sigma0*(1.0 + beta*log(abs(1.0 - q(i,j,qf)/f_inf)))


    q(i,j,qu)= 0.0 !cavity_speed*y/xmax(2)
    q(i,j,qv)= 0.0
  end do
end do

! Initialize old crap
q(:,:,qus) = q(:,:,qu)
q(:,:,qvs) = q(:,:,qv)
q(:,:,qum) = q(:,:,qu)
q(:,:,qvm) = q(:,:,qv)
q(:,:,qcm) = q(:,:,qc)
! q(:,:,qf2) = q(:,:,qf)
! q(:,:,qf2m) = q(:,:,qf)
!
end subroutine QInit2D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
subroutine QInit3D(q,mx,nrvars,h,xlower)
use NodeInfoDef
use GridUtilities, only: ULap3D
use ProblemDef
implicit none
!
real, dimension(1:,1:,1:,1:), intent(out):: q
integer, dimension(1:3), intent(in):: mx
integer, intent(in):: nrvars
real, intent(in):: h
real, dimension(1:3), intent(in):: xlower
!
integer:: i, j, k
real:: delta, r1, r2, r3, r4, tmp, x, y, z
real :: xc,yc,zc,r
!
!

! Circle parameters
xc = 4.0
yc = 4.0
zc = 4.0
r = 1.0

do i = 1, mx(1)
  x = (real(i,kind=r8)-0.5)*h+xlower(1)
  do j = 1, mx(2)
    y = (real(j,kind=r8)-0.5)*h+xlower(2)
    do k = 1, mx(3)
      z = (real(k,kind=r8)-0.5)*h+xlower(3)

      r1 = sqrt((x-xc)**2+(y-yc)**2)

    end do
  end do
end do
!
end subroutine QInit3D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
subroutine AfterStep2D(q,qparent,mx,nrvars,h,xlower,level)
use NodeInfoDef
use ProblemDef
implicit none
!
real, dimension(0:,0:,1:), intent(inout):: q
real, dimension(0:,0:,1:), intent(in):: qparent
integer, dimension(1:2), intent(in):: mx
integer, intent(in):: nrvars
real, intent(in):: h
real, dimension(1:2), intent(in):: xlower
integer, intent(in):: level
!
integer:: tmp
integer, dimension(1:2):: cmx
real:: ch, ch2, h2
!
cmx(1:2) = mx(1:2)/2
h2 = h*h; ch = h*2.0; ch2 = ch*ch
!

if(level==0) then
  integralresult(1) &
    = integralresult(1)+h2*sum(q(1:mx(1),1:mx(2),1))
else
  integralresult(1) &
    = integralresult(1)+h2*sum(q( 1: mx(1),1: mx(2),1)) &
    -                  ch2*sum(qparent(1:cmx(1),1:cmx(2),1))
end if

end subroutine AfterStep2D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
subroutine AfterStep3D(q,qparent,mx,nrvars,h,xlower,level)
use NodeInfoDef
use ProblemDef
implicit none
!
real, dimension(0:,0:,0:,1:), intent(in):: q
real, dimension(0:,0:,0:,1:), intent(in):: qparent
integer, dimension(1:3), intent(in):: mx
integer, intent(in):: nrvars
real, intent(in):: h
real, dimension(1:3), intent(in):: xlower
integer, intent(in):: level
!
integer:: tmp
integer, dimension(1:3):: cmx
real:: ch, ch3, h3
!
cmx(1:3) = mx(1:3)/2
h3 = h*h*h; ch = h*2.0; ch3 = ch*ch*ch
!
if(level==0) then
  integralresult(1) &
    = integralresult(1)+h3*sum(q(1:mx(1),1:mx(2),1:mx(3),1))
else
  integralresult(1) &
    = integralresult(1)+h3*sum(q( 1: mx(1),1: mx(2),1: mx(3),1)) &
    -                  ch3*sum(qparent(1:cmx(1),1:cmx(2),1:cmx(3),1))
end if
!
end subroutine AfterStep3D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!    
subroutine SetAux(info)
use NodeInfoDef
use ProblemDef
implicit none
!
TYPE(nodeinfo):: info
!
integer:: i, j, k
real:: time, x, y, z
!
!time = info%time
!
end subroutine SetAux
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!    
subroutine SetSrc(info)
use NodeInfoDef
use ProblemDef
implicit none
!    
TYPE(nodeinfo):: info
!
integer:: i, j, k
real:: time, x, y, z
!
!time = info%time
!
end subroutine SetSrc
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
subroutine SetErrFlagsUser2D(qrte,qnew,errorflags,mx,cmx,nrvars,h,xlower,level)
use ProblemDef
use NodeInfoDef
use GridUtilities, only: ULap2D
implicit none
!
real, dimension(1:,1:,1:), intent(in):: qrte
real, dimension(0:,0:,1:), intent(in):: qnew
integer, dimension(1:,1:), intent(out):: errorflags
integer, dimension(1:2), intent(in):: mx
integer, dimension(1:2), intent(in):: cmx
integer, intent(in):: nrvars
real, intent(in):: h
real, dimension(1:2), intent(in):: xlower
integer, intent(in):: level
!
real, dimension(0:mx(1)+1,0:mx(2)+1):: tmp
!
errorflags(1:mx(1),1:mx(2)) = 0


! errorflags(mx(1)/2-mx(1)/4:mx(1)/2+mx(1)/4,mx(2)/2:mx(2)) = 1
! errorflags(10:20,10:20) = 1
!
! tmp(0:mx(1)+1,0:mx(2)+1) = qnew(0:mx(1)+1,0:mx(2)+1,qv) 
!
! where(abs(sqrt( &
!                ULap2D(qnew(0:mx(1)+1,0:mx(2)+1,qu))**2 &
!               +ULap2D(qnew(0:mx(1)+1,0:mx(2)+1,qv))**2 &
!               ))>10.0) 
!       errorflags(1:mx(1),1:mx(2)) = 1
! end where

! where(abs(qnew(0:mx(1)+1,0:mx(2)+1,qv))>15.0) 
!       errorflags(1:mx(1),1:mx(2)) = 1
! end where


! Error tagging based on concentration field
tmp(0:mx(1)+1,0:mx(2)+1) = qnew(0:mx(1)+1,0:mx(2)+1,qc) 

! Cover the drop interface with fine grid
where(abs(sqrt(  (tmp(2:mx(1)+1,1:mx(2)) &
               -  tmp(0:mx(1)-1,1:mx(2)))**2 &
               + (tmp(1:mx(1),2:mx(2)+1) &
               -  tmp(1:mx(1),0:mx(2)-1))**2)) &
               > qtolerance(level))
      errorflags(1:mx(1),1:mx(2)) = 1
end where

! Cover the entire drop with fine grid
! where(tmp(1:mx(1),1:mx(2)) > 0.01 ) 
!       errorflags(1:mx(1),1:mx(2)) = 1
! end where

end subroutine SetErrFlagsUser2D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
subroutine SetErrFlagsUser3D(qrte,qnew,errorflags,mx,cmx,nrvars,h,xlower,level)
use NodeInfoDef
use ProblemDef
implicit none
!
real, dimension(1:,1:,1:,1:), intent(in):: qrte
real, dimension(0:,0:,0:,1:), intent(in):: qnew
integer, dimension(1:,1:,1:), intent(out):: errorflags
integer, dimension(1:3), intent(in):: mx
integer, dimension(1:3), intent(in):: cmx
integer, intent(in):: nrvars
real, intent(in):: h
real, dimension(1:3), intent(in):: xlower
integer, intent(in):: level
!
real, dimension(0:mx(1)+1,0:mx(2)+1,0:mx(3)+1):: tmp
!
errorflags(1:mx(1),1:mx(2),1:mx(3)) = 0
!
tmp(0:mx(1)+1,0:mx(2)+1,0:mx(3)+1) = qnew(0:mx(1)+1,0:mx(2)+1,0:mx(3)+1,qp) 
!
! Error tagging based on the undivided gradient of the viable cell density:
where(abs(sqrt(  (tmp(2:mx(1)+1,1:mx(2),1:mx(3)) &
               -  tmp(0:mx(1)-1,1:mx(2),1:mx(3)))**2 &
               + (tmp(1:mx(1),2:mx(2)+1,1:mx(3)) &
               -  tmp(1:mx(1),0:mx(2)-1,1:mx(3)))**2 &
               + (tmp(1:mx(1),1:mx(2),2:mx(3)+1) &
               -  tmp(1:mx(1),1:mx(2),0:mx(3)-1))**2)) &
               > qtolerance(level))
      errorflags(1:mx(1),1:mx(2),1:mx(3)) = 1
end where
!
end subroutine SetErrFlagsUser3D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
! User-Supplied AFAS Multigrid Routines:
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
! 2D Multigrid Routines:
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
subroutine RelaxGrid2D(qnew,qold,aux,f,mx,nrvars,maux,h,xlower,ipass)
! Red-black Gauss-Seidel iteration of the entire system
use NodeInfoDef
use ProblemDef
implicit none
!
real, dimension(0:,0:,1:), intent(in out):: qnew
real, dimension(0:,0:,1:), intent(in):: qold
real, dimension(0:,0:,1:), intent(in):: aux
real, dimension(1:,1:,1:), intent(in):: f
integer, dimension(1:2), intent(in):: mx
integer, intent(in):: nrvars
integer, intent(in):: maux
real, intent(in):: h
real, dimension(1:2), intent(in):: xlower
real, dimension(1:mx(1),1:mx(2)) :: gradp
real, dimension(0:mx(1),0:mx(2)) :: flux
integer, intent(in):: ipass
!
real, dimension(0:mx(1)+1,0:mx(2)+1) :: rhos, mus
integer:: i, j
real:: m1, m2, m3, m4, det, h2, mobij, muij
real:: dt_2h2,sdt_2h2,foureps2_h2,eps2_h2, &
       src,dt_2,bdt_2h2,dt_2h,sdt_2,bdt_2,invh2
real :: cij,c1,c2,c3,c4,neumann,denom
real :: dij,d1,d2,d3,d4
real :: rij,r1,r2,r3,r4
real, dimension(1:2):: b
real, dimension(1:2,1:2):: a = 1.0
real :: x=0.0,axisym_contrib=0.0,one_2hx
real, dimension(qus:qvs) :: axisym_contrib2 = 0.0, axisym_contrib3 = 0.0

! TODO switch statement here too? Not really necessary...
h2 = h*h
dt_2 = 0.5*dt
dt_2h = dt_2/h
dt_2h2 = dt_2/h2
invh2 = 1.0/h2
eps2_h2 = eps2/h2
foureps2_h2 = 4.0*eps2_h2
bdt_2 = bulkdiff*dt_2
bdt_2h2 = bulkdiff*dt_2h2
sdt_2 = surfdiff*dt_2
sdt_2h2 = surfdiff*dt_2h2

! TODO. Hmm, this will get called two times bc of red/black.
! Still faster? At least still cleaner...
if(eqtag == eq_mac) then
  rhos = rho(1)!1.0/SmearHarm(rho(1),rho(2),qold(0:mx(1)+1,0:mx(2)+1,qc))
elseif(eqtag == eq_projection) then
  rhos = rho(1)!1.0!/SmearHarm(rho(1),rho(2),0.5*(qnew(0:mx(1)+1,0:mx(2)+1,qc) &
            !                        + qold(0:mx(1)+1,0:mx(2)+1,qc)))
endif

do j = 1, mx(2)
  do i = 1+modulo(j+ipass,2), mx(1), 2
    if(axisym) x = xlower(1)+(real(i)-0.5)*h; one_2hx=1.0/(2.0*h*x)

    select case(eqtag)
    case(eq_mac)

      ! Densities at cell edges. Harmonic interpolation
!       rij =     rhos(i,j)
!       r1 = 0.5*(rhos(i-1,j)+rij)
!       r2 = 0.5*(rhos(i+1,j)+rij)
!       r3 = 0.5*(rhos(i,j-1)+rij)
!       r4 = 0.5*(rhos(i,j+1)+rij)

      if(axisym) then
        axisym_contrib = (qnew(i+1,j,qmac)-qnew(i-1,j,qmac))*one_2hx
      endif

      qnew(i,j,qmac) = 0.25*(h2*rho(1)*(axisym_contrib-f(i,j,qmac))& 
                 + (qnew(i-1,j,qmac) + qnew(i+1,j,qmac)     &
                 +  qnew(i,j-1,qmac) + qnew(i,j+1,qmac)))

    case(eq_ch)

      ! Mobilities at cell edges
      mobij =   Mob(qnew(i,j,qc))
      m1 = 0.5*(Mob(qnew(i-1,j,qc))+mobij)
      m2 = 0.5*(Mob(qnew(i+1,j,qc))+mobij)
      m3 = 0.5*(Mob(qnew(i,j-1,qc))+mobij)
      m4 = 0.5*(Mob(qnew(i,j+1,qc))+mobij)

      ! Use Cramer's rule to invert the concentration/chemical pot. system
      a(1,2) = dt_2h2*(m1+m2+m3+m4)
      a(2,1) = -(foureps2_h2 + D2f(qnew(i,j,qc)))

      if(axisym) then
        axisym_contrib = dt_2*mobij*(qnew(i+1,j,qmu)-qnew(i-1,j,qmu))*one_2hx
      endif

      b(1) = f(i,j,qc)+dt_2h2*(m1*qnew(i-1,j,qmu)+m2*qnew(i+1,j,qmu) &
           +                   m3*qnew(i,j-1,qmu)+m4*qnew(i,j+1,qmu))&
           + axisym_contrib

      if(axisym) then
        axisym_contrib = eps2*(qnew(i+1,j,qc)-qnew(i-1,j,qc))*one_2hx
      endif

      b(2) = f(i,j,qmu)+Df(qnew(i,j,qc))-D2f(qnew(i,j,qc))*qnew(i,j,qc) &
           - eps2_h2*(qnew(i+1,j,qc)+qnew(i-1,j,qc)                     &
                     +qnew(i,j+1,qc)+qnew(i,j-1,qc))                    &
           - axisym_contrib

      det = 1.0-a(1,2)*a(2,1)

      qnew(i,j,qc)  = (b(1)-a(1,2)*b(2))/det
      qnew(i,j,qmu) = (b(2)-a(2,1)*b(1))/det

    case(eq_surfac)

      !- Bulk surfactant-equation -!

      ! Concentration at cell edges
      cij = Heavi(qnew(i,j,qc))
      c1 = 0.5*(Heavi(qnew(i-1,j,qc))+cij)
      c2 = 0.5*(Heavi(qnew(i+1,j,qc))+cij)
      c3 = 0.5*(Heavi(qnew(i,j-1,qc))+cij)
      c4 = 0.5*(Heavi(qnew(i,j+1,qc))+cij)

      neumann = dt_2h*Delta2(qnew(i-1:i+1,j-1:j+1,qc))

      if(axisym) then
        axisym_contrib = bdt_2*cij*(qnew(i+1,j,qb)-qnew(i-1,j,qb))*one_2hx
      endif

      ! Implicit treatment of sourceterm
!       denom = (cij + neumann*r_a + bdt_2h2*(c1+c2+c3+c4))
!       a(1,2) = -neumann*r_d/denom
      ! Explicit treatment of sourceterm
      denom = (cij + bdt_2h2*(c1+c2+c3+c4))
      a(1,2) = 0.0
      b(1) = (f(i,j,qb)                                      &
           + bdt_2h2*(c1*qnew(i-1,j,qb)+c2*qnew(i+1,j,qb)    &
           +          c3*qnew(i,j-1,qb)+c4*qnew(i,j+1,qb))   &
           + axisym_contrib)/denom      
                    
      !- Interface surfactant-equation -!

      ! Delta function at cell edges 
      dij =   Delta(qnew(i,j,qc))
      d1 = 0.5*(Delta(qnew(i-1,j,qc))+dij)
      d2 = 0.5*(Delta(qnew(i+1,j,qc))+dij)
      d3 = 0.5*(Delta(qnew(i,j-1,qc))+dij)
      d4 = 0.5*(Delta(qnew(i,j+1,qc))+dij)

      if(axisym) then
        axisym_contrib = sdt_2*dij*(qnew(i+1,j,qf)-qnew(i-1,j,qf))*one_2hx
      endif

!       denom = (dij + r_d*dij*dt_2 + sdt_2h2*(d1+d2+d3+d4))
!       a(2,1) = -r_a*dij*dt_2/denom
      denom = (dij + sdt_2h2*(d1+d2+d3+d4))
      a(2,1) = 0.0
      b(2) = (f(i,j,qf)                                    &
           + sdt_2h2*(d1*qnew(i-1,j,qf)+d2*qnew(i+1,j,qf)  &
           +          d3*qnew(i,j-1,qf)+d4*qnew(i,j+1,qf)) &
           + axisym_contrib)/denom
                    
      det = 1.0-a(1,2)*a(2,1)

      qnew(i,j,qb) = (b(1)-a(1,2)*b(2))/det
      qnew(i,j,qf) = (b(2)-a(2,1)*b(1))/det

      ! Second attempt at interface eq.
!       qnew(i,j,qf2) = f(i,j,qf2)                                    

    case(eq_advecdiff)

!       rij = 1.0/SmearHarm(rho(1),rho(2),0.5*(qnew(i,j,qc) + qold(i,j,qc)))

      muij  = SmearHarm(mu(1),mu(2),qnew(i,j,qc))
!       m1 = 0.5*(Smear(mu(1),mu(2),qnew(i-1,j,qc))+muij)
!       m2 = 0.5*(Smear(mu(1),mu(2),qnew(i+1,j,qc))+muij)
!       m3 = 0.5*(Smear(mu(1),mu(2),qnew(i,j-1,qc))+muij)
!       m4 = 0.5*(Smear(mu(1),mu(2),qnew(i,j+1,qc))+muij)

      if(axisym) then
        axisym_contrib2(qus:qvs) = dt_2*muij*(qnew(i+1,j,qus:qvs) &
                                 -            qnew(i-1,j,qus:qvs))*one_2hx
        ! Coriolis term
        axisym_contrib3 = (/ muij*dt_2/x**2, 0.0 /)
      endif
! div(mu*grad(u)
!       qnew(i,j,qus:qvs) =(f(i,j,qus:qvs) + axisym_contrib2(qus:qvs)      &
!           + rij*dt_2h2*(m1*qnew(i-1,j,qus:qvs) + m2*qnew(i+1,j,qus:qvs)   &
!           +             m3*qnew(i,j-1,qus:qvs) + m4*qnew(i,j+1,qus:qvs))) &
!           / (1.0 + (m1+m2+m3+m4)*rij*dt_2h2 + axisym_contrib)
! mu*Lap(u)
      qnew(i,j,qus:qvs) =(f(i,j,qus:qvs) + axisym_contrib2(qus:qvs)      &
          + muij*dt_2h2*(qnew(i-1,j,qus:qvs) + qnew(i+1,j,qus:qvs)   &
          +              qnew(i,j-1,qus:qvs) + qnew(i,j+1,qus:qvs))/rho(1)) &
          / (1.0 + 4.0*muij*dt_2h2/rho(1) + axisym_contrib3(qus:qvs))

      qnew(i,j,qsig:qmar)=f(i,j,qsig:qmar)

    case(eq_projection)

      ! Densities at cell edges. Harmonic interpolation
!       rij =     rhos(i,j)
!       r1 = 0.5*(rhos(i-1,j)+rij)
!       r2 = 0.5*(rhos(i+1,j)+rij)
!       r3 = 0.5*(rhos(i,j-1)+rij)
!       r4 = 0.5*(rhos(i,j+1)+rij)

      if(axisym) then
        axisym_contrib = (qnew(i+1,j,qpsi)-qnew(i-1,j,qpsi))*one_2hx
      endif

      qnew(i,j,qpsi) = 0.25*(h2*rho(1)*(axisym_contrib - f(i,j,qpsi)) & 
                 + qnew(i-1,j,qpsi) + qnew(i+1,j,qpsi)         &
                 + qnew(i,j-1,qpsi) + qnew(i,j+1,qpsi)) 

    case(eq_update) 

      qnew(i,j,qu:qvm) = f(i,j,qu:qvm)

      qnew(i,j,qcm) = f(i,j,qcm)

      qnew(i,j,qbm:qfm) = f(i,j,qbm:qfm)

!       max_p = max(qnew(i,j,qp),max_p)
!       min_p = min(qnew(i,j,qp),min_p)
!       max_psi = max(qnew(i,j,qpsi),max_psi)
!       min_psi = min(qnew(i,j,qpsi),min_psi)
      qnew(i,j,qdiv) = f(i,j,qdiv)
    case(eq_normalize)

      qnew(i,j,qp) = f(i,j,qp)
      qnew(i,j,qpsi) = f(i,j,qpsi)

    case default
      stop "Invalid eqtag in Relax2D!"
    end select

  end do
end do

if(eqtag == eq_update .and. mx(1)==32 &
                      .and. mx(2)==16 &
                      .and. xlower(1)==-4.0) then
!   print*, 'ugly, stupid hack',max_p
  max_psi = qnew(1,1,qpsi)
  max_p = qnew(1,1,qp)
endif
!
end subroutine RelaxGrid2D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
subroutine UpdateAuxVcycle2D(qnew,qold,aux,ll,mx,nrvars,maux,mbc,h,xlower)
use NodeInfoDef
use ProblemDef
implicit none
!
real, dimension(ll:,ll:,1:), intent(in):: qnew
real, dimension(ll:,ll:,1:), intent(in):: qold
real, dimension(ll:,ll:,1:), intent(in out):: aux
integer, intent(in):: ll
integer, dimension(1:2), intent(in):: mx
integer, intent(in):: nrvars
integer, intent(in):: maux
integer, intent(in):: mbc
real, intent(in):: h
real, dimension(1:2), intent(in):: xlower
!


end subroutine UpdateAuxVcycle2D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
function Operator2D(qnew,qold,aux,mx,nrvars,maux,h,xlower) result(opresult)
use NodeInfoDef
use GridUtilities, only: ULap2D
use ProblemDef
implicit none
!
real, dimension(0:,0:,1:), intent(in):: qnew
real, dimension(0:,0:,1:), intent(in):: qold
real, dimension(0:,0:,1:), intent(in):: aux
integer, dimension(1:2), intent(in):: mx
integer, intent(in):: nrvars
integer, intent(in):: maux
real, intent(in):: h
real, dimension(1:2), intent(in):: xlower
real, dimension(1:mx(1),1:mx(2),1:nrvars):: opresult
!
real, dimension(1:mx(1),1:mx(2)):: src,mus,xs
real, dimension(0:mx(1)+1,0:mx(2)+1):: rhos,ch,musvar
real:: dt_2, dt_2h, h2, sdt_2, bdt_2
integer :: k

opresult = 0.0
dt_2 = 0.5*dt

select case(eqtag)
case(eq_mac)

!   rhos = 1.0/SmearHarm(rho(1),rho(2),qold(0:mx(1)+1,0:mx(2)+1,qc))

  opresult(1:mx(1),1:mx(2),qmac) &
    = Lap2D(qnew(0:mx(1)+1,0:mx(2)+1,qmac),xlower,h)/rho(1)
!     = LapVar2D(rhos,qnew(0:mx(1)+1,0:mx(2)+1,qmac),xlower,h)

case(eq_ch)

  opresult(1:mx(1),1:mx(2),qc)                          &
    = qnew(1:mx(1),1:mx(2),qc)                          &
    - dt_2*LapVar2D(Mob(qnew(0:mx(1)+1,0:mx(2)+1,qc)),  &
                        qnew(0:mx(1)+1,0:mx(2)+1,qmu),xlower,h)

  opresult(1:mx(1),1:mx(2),qmu)                         &
    = qnew(1:mx(1),1:mx(2),qmu)                         &
    - Df(qnew(1:mx(1),1:mx(2),qc))                      &
    + eps2*Lap2D(qnew(0:mx(1)+1,0:mx(2)+1,qc),xlower,h)

case(eq_surfac)

  sdt_2 = surfdiff*dt_2
  bdt_2 = bulkdiff*dt_2

!   src = r_a*qnew(1:mx(1),1:mx(2),qb) &!*(f_inf - qnew(1:mx(1),1:mx(2),qf))&
!       - r_d*qnew(1:mx(1),1:mx(2),qf)
  src = 0.0

  opresult(1:mx(1),1:mx(2),qf)                             &
    = Delta(qnew(1:mx(1),1:mx(2),qc))*                     &
           (qnew(1:mx(1),1:mx(2),qf) - dt_2*src)           &
    - sdt_2*LapVar2D(Delta(qnew(0:mx(1)+1,0:mx(2)+1,qc)),  &
                     qnew(0:mx(1)+1,0:mx(2)+1,qf),         &
                     xlower,h)

  opresult(1:mx(1),1:mx(2),qb)                             &
    = Heavi(qnew(1:mx(1),1:mx(2),qc))                      &
    *       qnew(1:mx(1),1:mx(2),qb)                       &
    + dt_2*Delta(qnew(1:mx(1),1:mx(2),qc))*src       &
    - bdt_2*LapVar2D(Heavi(qnew(0:mx(1)+1,0:mx(2)+1,qc)),  &
                     qnew(0:mx(1)+1,0:mx(2)+1,qb),         &
                     xlower,h)

!   opresult(1:mx(1),1:mx(2),qf2)                             &
!     = qnew(1:mx(1),1:mx(2),qf2) 

case(eq_advecdiff)

!   rhos = 1.0/SmearHarm(rho(1),rho(2),0.5*(qnew(0:mx(1)+1,0:mx(2)+1,qc) &
!                                         + qold(0:mx(1)+1,0:mx(2)+1,qc)))

  mus = SmearHarm(mu(1),mu(2),qnew(1:mx(1),1:mx(2),qc))
!     musvar = Smear(mu(1),mu(2),qnew(0:mx(1)+1,0:mx(2)+1,qc))

  do k=qus,qvs
    opresult(1:mx(1),1:mx(2),k) &
      = qnew(1:mx(1),1:mx(2),k) &
      - dt_2*mus*Lap2D(qnew(0:mx(1)+1,0:mx(2)+1,k),xlower,h)/rho(1)
!         - dt_2*LapVar2D(musvar,qnew(0:mx(1)+1,0:mx(2)+1,k),xlower,h)&
!       * rhos(1:mx(1),1:mx(2))
  enddo

  if(axisym) then
    ! Add Coriolis term, +u/x^2, to x-momentum equation
    xs = GetX(xlower,mx,h)
    opresult(1:mx(1),1:mx(2),qus) = opresult(1:mx(1),1:mx(2),qus) &
      + dt_2*mus*qnew(1:mx(1),1:mx(2),qus)*rhos(1:mx(1),1:mx(2))/xs**2
  endif

  opresult(1:mx(1),1:mx(2),qsig:qmar) = qnew(1:mx(1),1:mx(2),qsig:qmar)

case(eq_projection)

!   rhos = 1.0/SmearHarm(rho(1),rho(2),0.5*(qnew(0:mx(1)+1,0:mx(2)+1,qc) &
!                                         + qold(0:mx(1)+1,0:mx(2)+1,qc)))
  opresult(1:mx(1),1:mx(2),qpsi) &
    = Lap2D(qnew(0:mx(1)+1,0:mx(2)+1,qpsi),xlower,h)/rho(1)
!     = LapVar2D(rhos,qnew(0:mx(1)+1,0:mx(2)+1,qpsi),xlower,h)

case(eq_update)

  opresult(1:mx(1),1:mx(2),qu:qvm) = qnew(1:mx(1),1:mx(2),qu:qvm)

  opresult(1:mx(1),1:mx(2),qcm) = qnew(1:mx(1),1:mx(2),qcm)

  opresult(1:mx(1),1:mx(2),qbm:qfm) = qnew(1:mx(1),1:mx(2),qbm:qfm)

  opresult(1:mx(1),1:mx(2),qdiv) = qnew(1:mx(1),1:mx(2),qdiv)
case(eq_normalize)

  opresult(1:mx(1),1:mx(2),qp) = qnew(1:mx(1),1:mx(2),qp)

  opresult(1:mx(1),1:mx(2),qpsi) = qnew(1:mx(1),1:mx(2),qpsi)

end select

end function Operator2D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
function Source2D(qnew,qold,aux,ll,mx,nrvars,maux,mbc,h,xlower) &
         result(srcresult)
use NodeInfoDef
use ProblemDef
implicit none
!
real, dimension(ll:,ll:,1:), intent(in):: qnew
real, dimension(ll:,ll:,1:), intent(in):: qold
real, dimension(ll:,ll:,1:), intent(in):: aux
integer, intent(in):: ll
integer, dimension(1:2), intent(in):: mx
integer, intent(in):: nrvars
integer, intent(in):: maux
integer, intent(in):: mbc
real, intent(in):: h
real, dimension(1:2), intent(in):: xlower
real, dimension(1:mx(1),1:mx(2),1:nrvars):: srcresult
!
real, dimension(0:mx(1),0:mx(2),2):: flux
real, dimension(1:mx(1),1:mx(2),qus:qvs):: gradp,surften_force
real, dimension(1:mx(1),1:mx(2),qu:qv):: gradpsi
real, dimension(0:mx(1), 0:mx(2), qus:qvs):: ue
real, dimension(0:mx(1), 1:mx(2)):: uhe
real, dimension(1:mx(1), 0:mx(2)):: vhe
real, dimension(-1:mx(1)+2,-1:mx(2)+2,qus:qvs):: uh
real, dimension(-1:mx(1)+2,-1:mx(2)+2):: ch,muh,fh,bh,fh2
real, dimension(0:mx(1),0:mx(2),1:2):: gradmac
real, dimension(0:mx(1),0:mx(2)):: tmp
real, dimension(0:mx(1)+1,0:mx(2)+1):: rhos,normpsi,musvar,sigma,xs2
real, dimension(1:mx(1),1:mx(2)):: mus,xs,src
real :: h2,dt_2,dt_h,bdt_2,sdt_2,theta,x,y,xc,yc,dt_2h
real :: divu,invhdt
real :: p11
integer :: i,j,k

srcresult = 0.0

dt_2 = 0.5*dt
dt_h = dt/h

if(eqtag==eq_ch .or. eqtag==eq_surfac .or. eqtag==eq_advecdiff) then

  ! Extrapolate to find velocities at half step
  uh = 0.5*(3.0*qold(-1:mx(1)+2,-1:mx(2)+2,qu:qv) &
     -          qold(-1:mx(1)+2,-1:mx(2)+2,qum:qvm))

  ! Interpolate to edges
  uhe = 0.5*(uh(0:mx(1),1:mx(2),qus) + uh(1:mx(1)+1,1:mx(2),  qus))
  vhe = 0.5*(uh(1:mx(1),0:mx(2),qvs) + uh(1:mx(1),  1:mx(2)+1,qvs))

  ! Apply MAC projection to get divergence free fields
  gradmac = EGrad2D(qnew(0:mx(1)+1,0:mx(2)+1,qmac),h)
  uhe = uhe - gradmac(0:mx(1),1:mx(2),1)/rho(1)
  vhe = vhe - gradmac(1:mx(1),0:mx(2),2)/rho(1)

endif

select case(eqtag)
case(eq_mac)

  ! Extrapolate to find velocities at half step
  uh = 0.5*(3.0*qold(-1:mx(1)+2,-1:mx(2)+2,qu:qv) &
     -          qold(-1:mx(1)+2,-1:mx(2)+2,qum:qvm))

  ! Interpolate to edges
  ue(0:mx(1),1:mx(2),qus) = 0.5*(uh(0:mx(1),  1:mx(2),qus) &
                          +      uh(1:mx(1)+1,1:mx(2),qus))
  ue(1:mx(1),0:mx(2),qvs) = 0.5*(uh(1:mx(1),0:mx(2),  qvs) &
                          +      uh(1:mx(1),1:mx(2)+1,qvs))

  srcresult(1:mx(1),1:mx(2),qmac) = Div2D(ue,xlower,h)

case(eq_ch)

  ! Extrapolate to find concentration at half step
  ch = 0.5*(3.0*qold(-1:mx(1)+2,-1:mx(2)+2,qc) &
     -          qold(-1:mx(1)+2,-1:mx(2)+2,qcm))

  srcresult(1:mx(1),1:mx(2),qc)                                &
    = qold(1:mx(1),1:mx(2),qc)                                 &
    - dt_h*UWeno2D(ch(-1:mx(1)+2,-1:mx(2)+2),                  &
                   uhe(0:mx(1),1:mx(2)), vhe(1:mx(1),0:mx(2))) &
    + dt_2*LapVar2D(Mob(qold(0:mx(1)+1,0:mx(2)+1,qc)),         &
                    qold(0:mx(1)+1,0:mx(2)+1,qmu),xlower,h)

  srcresult(1:mx(1),1:mx(2),qmu) = 0.0 

case(eq_surfac)

  sdt_2 = surfdiff*dt_2
  bdt_2 = bulkdiff*dt_2

  ! Average to find concentration at half step
  ch = 0.5*(qnew(-1:mx(1)+2,-1:mx(2)+2,qc)+qold(-1:mx(1)+2,-1:mx(2)+2,qc))
  ! Extrapolate to find concentration at half step
  fh = 0.5*(3.0*qold(-1:mx(1)+2,-1:mx(2)+2,qf) &
     -          qold(-1:mx(1)+2,-1:mx(2)+2,qfm))
  bh = 0.5*(3.0*qold(-1:mx(1)+2,-1:mx(2)+2,qb) &
     -          qold(-1:mx(1)+2,-1:mx(2)+2,qbm))
!   fh2 = 0.5*(3.0*qold(-1:mx(1)+2,-1:mx(2)+2,qf2) &
!       -          qold(-1:mx(1)+2,-1:mx(2)+2,qf2m))

  ! Interpolate to edges
!   ue(0:mx(1),1:mx(2),1) = 0.5*(qold(0:mx(1),  1:mx(2),qu) &
!                           +    qold(1:mx(1)+1,1:mx(2),qu))
!   ue(1:mx(1),0:mx(2),2) = 0.5*(qold(1:mx(1),0:mx(2),  qv) &
!                           +    qold(1:mx(1),1:mx(2)+1,qv))

  ! Apply MAC projection to get divergence free fields
!   gradmac = EGrad2D(qnew(0:mx(1)+1,0:mx(2)+1,qmac),h)
!   ue(0:mx(1),1:mx(2),1) = ue(0:mx(1),1:mx(2),1) - gradmac(0:mx(1),1:mx(2),1)
!   ue(0:mx(1),1:mx(2),2) = ue(0:mx(1),1:mx(2),1) - gradmac(1:mx(1),0:mx(2),2)

  src = r_a*qold(1:mx(1),1:mx(2),qb)*(f_inf - qold(1:mx(1),1:mx(2),qf))&
      - r_d*qold(1:mx(1),1:mx(2),qf)
!   src = r_a*bh(1:mx(1),1:mx(2)) & !*(f_inf - fh(1:mx(1),1:mx(2)))&
!       - r_d*fh(1:mx(1),1:mx(2))

  srcresult(1:mx(1),1:mx(2),qf)                           &
    = Delta(qold(1:mx(1),1:mx(2),qc))*                    &
           (qold(1:mx(1),1:mx(2),qf) + dt_2*src)          &
!     - 0.5*dt_h*UWeno2D(Delta(qold(-1:mx(1)+2,-1:mx(2)+2,qc))&
!     *qold(-1:mx(1)+2,-1:mx(2)+2,qf),                        &
!                    ue(0:mx(1),1:mx(2),1),                  &
!                    ue(1:mx(1),0:mx(2),2))                  &
    - dt_h*UWeno2D(Delta(ch)*fh, &
                   uhe(0:mx(1),1:mx(2)),                  &
                   vhe(1:mx(1),0:mx(2)))                  &
    + sdt_2*LapVar2D(Delta(qold(0:mx(1)+1,0:mx(2)+1,qc)), &
                     qold(0:mx(1)+1,0:mx(2)+1,qf),        &
                     xlower,h) 

!   srcresult(1:mx(1),1:mx(2),qf2)                       &
!     = qold(1:mx(1),1:mx(2),qf2)                           &
!     - dt_2*(UWeno2D(qold(-1:mx(1)+2,-1:mx(2)+2,qf2),       &
!                     ue(0:mx(1),1:mx(2),1),             &
!                     ue(1:mx(1),0:mx(2),2))/h             &
!     -      NGradUN2D(Normal2D(qold(0:mx(1)+1,0:mx(2)+1,qc),h),&
!                      qold(0:mx(1)+1,0:mx(2)+1,qu), &
!                      qold(0:mx(1)+1,0:mx(2)+1,qv),h)&
!                     *qold(-1:mx(1)+2,-1:mx(2)+2,qf2))&
!     + sdt_2*LapSurf2D(qold(0:mx(1)+1,0:mx(2)+1,qc),       &
!                       qold(0:mx(1)+1,0:mx(2)+1,qf2),      &
!                       xlower,h) 

  srcresult(1:mx(1),1:mx(2),qb)                           &
    = Heavi(qold(1:mx(1),1:mx(2),qc))                     &
           *qold(1:mx(1),1:mx(2),qb)                      &
    - Delta(qold(1:mx(1),1:mx(2),qc))*dt_2*src      &
    - dt_h*UWeno2D(Heavi(ch)*bh,                          &
                   uhe(0:mx(1),1:mx(2)),                  &
                   vhe(1:mx(1),0:mx(2)))                  &
    + bdt_2*LapVar2D(Heavi(qold(0:mx(1)+1,0:mx(2)+1,qc)), &
                     qold(0:mx(1)+1,0:mx(2)+1,qb),        &
                     xlower,h) 

case(eq_advecdiff)

  ! Average to find concentration at half step
  ch = 0.5*(qnew(-1:mx(1)+2,-1:mx(2)+2,qc)+qold(-1:mx(1)+2,-1:mx(2)+2,qc))
  fh = 0.5*(qnew(-1:mx(1)+2,-1:mx(2)+2,qf)+qold(-1:mx(1)+2,-1:mx(2)+2,qf))

  ! Langmuir equation of state (linear and non-linear)
!  where (fh(1:mx(1),1:mx(2))/f_inf < 1.0) 
!    sigma = sigma0*(1.0 + beta*log(1.0 - fh(0:mx(1)+1,0:mx(2)+1)/f_inf))
!  elsewhere
!    sigma = 0.001
!  end where
  ! linear
  sigma = sigma0*(1.0 + beta*log(abs(1.0-fh(0:mx(1)+1,0:mx(2)+1)/f_inf)))
!   sigma = sigma0*(1.0 - beta*fh(0:mx(1)+1,0:mx(2)+1))
!   if(any(sigma < 0.0)) print*, 'Warning, negative surface tension!'
!   where (sigma < 0.0)
!     sigma = 0.001
!   end where
!   sigma = 2.0 + (GetX(xlower,mx,h) + GetY(xlower,mx,h))

  srcresult(1:mx(1),1:mx(2),qsig) = sigma(1:mx(1),1:mx(2))

  surften_force = SurfTen2D(ch(0:mx(1)+1,0:mx(2)+1),sigma,xlower,h)

  srcresult(1:mx(1),1:mx(2),qcap) = Cap2D(ch(0:mx(1)+1,0:mx(2)+1),sigma,xlower,h)
  srcresult(1:mx(1),1:mx(2),qmar) = Mar2D(ch(0:mx(1)+1,0:mx(2)+1),sigma,xlower,h)
!   srcresult(1:mx(1),1:mx(2),qcap:qmar) = Normal2D(ch(0:mx(1)+1,0:mx(2)+1),h)

  mus = SmearHarm(mu(1),mu(2),qold(1:mx(1),1:mx(2),qc))
!   musvar = Smear(mu(1),mu(2),qold(0:mx(1)+1,0:mx(2)+1,qc))

  gradp = Grad2D(qold(0:mx(1)+1,0:mx(2)+1,qp),h)

  do k=qus,qvs
    srcresult(1:mx(1),1:mx(2),k)                                   &
      = qold(1:mx(1),1:mx(2),k+old)                                &
      - dt_h*UWeno2D(uh(-1:mx(1)+2,-1:mx(2)+2,k),                  &
                     uhe(0:mx(1),1:mx(2)), vhe(1:mx(1),0:mx(2)))   &
      + dt*(0.5*mus*Lap2D(qold(0:mx(1)+1,0:mx(2)+1,k+old),xlower,h)&
!       + dt*(0.5*LapVar2D(musvar,qold(0:mx(1)+1,0:mx(2)+1,k+old),xlower,h)&
      + -gradp(:,:,k) + surften_force(:,:,k))/rho(1) 
!       + (1.0-rho(2)*rhos(1:mx(1),1:mx(2)))*dt*g(k)
  enddo

  if(axisym) then
  ! Add Coriolis term, -u/x^2, to x-momentum equation
    xs = GetX(xlower,mx,h)
    srcresult(1:mx(1),1:mx(2),qus) = srcresult(1:mx(1),1:mx(2),qus) &
      - mus*dt_2*qold(1:mx(1),1:mx(2),qu)/xs**2
  endif

case(eq_projection)

!   srcresult(1:mx(1),1:mx(2),qpsi) &
!     = 1.0*DivRhieChow2D(qnew(-1:mx(1)+1,-1:mx(2)+1,qus:qvs) &
!                    -0.0*qold(-1:mx(1)+1,-1:mx(2)+1,qu:qv), &
!                     qold(-1:mx(1)+2,-1:mx(2)+2,qp), &
! !                   1.0/SmearHarm(rho(1),rho(2),qold(-1:mx(1)+1,-1:mx(2)+1,qc)), &
!                     xlower,h)/dt
  ! Interpolate to edges
  ue(0:mx(1),1:mx(2),qus) = 0.5*(qnew(0:mx(1),  1:mx(2),qus) &
                          +      qnew(1:mx(1)+1,1:mx(2),qus))
  ue(1:mx(1),0:mx(2),qvs) = 0.5*(qnew(1:mx(1),0:mx(2),  qvs) &
                          +      qnew(1:mx(1),1:mx(2)+1,qvs))

!   srcresult(1:mx(1),1:mx(2),qpsi) = Div2D(ue,xlower,h)/dt 
  srcresult(1:mx(1),1:mx(2),qpsi) = CDiv2D(qnew(0:mx(1)+1,0:mx(2)+1,qus:qvs), &
                                           xlower,h)/dt 
  ! Apply MAC projection to get divergence free fields
!   gradmac = EGrad2D(qnew(0:mx(1)+1,0:mx(2)+1,qmac),h)
!   ue(0:mx(1),1:mx(2),1) = ue(0:mx(1),1:mx(2),1) - gradmac(0:mx(1),1:mx(2),1)
!   ue(1:mx(1),0:mx(2),2) = ue(1:mx(1),0:mx(2),2) - gradmac(1:mx(1),0:mx(2),2)


case(eq_update)

  gradpsi = Grad2D(qnew(0:mx(1)+1,0:mx(2)+1,qpsi),h)
!   rhos = 1.0/SmearHarm(rho(1),rho(2),0.5*(qnew(0:mx(1)+1,0:mx(2)+1,qc) &
!                                         + qold(0:mx(1)+1,0:mx(2)+1,qc)))

  do k=qu,qv
    srcresult(1:mx(1),1:mx(2),k) &
    = qnew(1:mx(1),1:mx(2),k-old)-1.0*dt*gradpsi(:,:,k)/rho(1) !*rhos(1:mx(1),1:mx(2))
  enddo

  srcresult(1:mx(1),1:mx(2),qp) &
    = qold(1:mx(1),1:mx(2),qp) + qnew(1:mx(1),1:mx(2),qpsi) &
    - 0.5*CDiv2D(qnew(0:mx(1)+1,0:mx(2)+1,qus:qvs),xlower,h)*mu(1)

! Only works on uniform grids
!   p11 = srcresult(1,mx(2),qp)
!   srcresult(1:mx(1),1:mx(2),qp) &
!     = srcresult(1:mx(1),1:mx(2),qp) - p11

  ! Update old variables
  srcresult(1:mx(1),1:mx(2),qum:qvm) &
    = qold(1:mx(1),1:mx(2),qu:qv) 

  srcresult(1:mx(1),1:mx(2),qcm) &
    = qold(1:mx(1),1:mx(2),qc) 

  srcresult(1:mx(1),1:mx(2),qbm:qfm) &
    = qold(1:mx(1),1:mx(2),qb:qf) 
  srcresult(1:mx(1),1:mx(2),qdiv) &
    = Cdiv2D(qnew(0:mx(1)+1,0:mx(2)+1,qus:qvs),xlower,h)
case(eq_normalize)
  ! This is so hacky I'm almost ashamed...

!  if (max_p == -1e5) then
!    print*, 'Did you remember the hack?'
!    read (*,*)
!  endif

  srcresult(1:mx(1),1:mx(2),qp) = qnew(1:mx(1),1:mx(2),qp) !- max_p

  srcresult(1:mx(1),1:mx(2),qpsi) = qnew(1:mx(1),1:mx(2),qpsi) !-max_psi
  
end select

end function Source2D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

function SourceUpdate2D(qnew,qold,aux,ll,mx,nrvars,maux,mbc,h,xlower) &
         result(updateresult)
use NodeInfoDef
use ProblemDef
implicit none
!
real, dimension(ll:,ll:,1:), intent(in):: qnew
real, dimension(ll:,ll:,1:), intent(in):: qold
real, dimension(ll:,ll:,1:), intent(in):: aux
integer, intent(in):: ll
integer, dimension(1:2), intent(in):: mx
integer, intent(in):: nrvars
integer, intent(in):: maux
integer, intent(in):: mbc
real, intent(in):: h
real, dimension(1:2), intent(in):: xlower
real, dimension(1:mx(1),1:mx(2),1:nrvars):: updateresult
real, dimension(1:mx(1),1:mx(2)) :: src
real, dimension(0:mx(1),0:mx(2),1:2):: gradmac
real, dimension(0:mx(1)  ,1:mx(2)  ):: uhe
real, dimension(1:mx(1)  ,0:mx(2)  ):: vhe
!
real:: dt_2,dt_2h,mudt_2rho

dt_2 = 0.5*dt
dt_2h = 0.5*dt/h

updateresult = 0.0

if(eqtag==eq_surfac) then

  src = r_a*qnew(1:mx(1),1:mx(2),qb)*(f_inf - qnew(1:mx(1),1:mx(2),qf))&
      - r_d*qnew(1:mx(1),1:mx(2),qf)

  updateresult(1:mx(1),1:mx(2),qf) &
    = dt_2*Delta(qnew(1:mx(1),1:mx(2),qc))*src 

  updateresult(1:mx(1),1:mx(2),qb) &
    =-dt_2*Delta(qnew(1:mx(1),1:mx(2),qc))*src  

endif
    
! if(eqtag == eq_surfac) then
! 
!   ! Interpolate to edges
!   uhe = 0.5*(qnew(0:mx(1),  1:mx(2),qu) &
!       +      qnew(1:mx(1)+1,1:mx(2),qu))
!   vhe = 0.5*(qnew(1:mx(1),0:mx(2),  qv) &
!       +      qnew(1:mx(1),1:mx(2)+1,qv))
! 
!   ! Apply MAC projection to get divergence free fields
! !   gradmac = EGrad2D(qnew(0:mx(1)+1,0:mx(2)+1,qmac),h)
! !   uhe = uhe - gradmac(0:mx(1),1:mx(2),1)
! !   vhe = vhe - gradmac(1:mx(1),0:mx(2),2)
! 
!   updateresult(1:mx(1),1:mx(2),qf) =                       &
!     - dt_2h*UWeno2D(Delta(qnew(-1:mx(1)+2,-1:mx(2)+2,qc))&
!                          *qnew(-1:mx(1)+2,-1:mx(2)+2,qf),&
!                     uhe(0:mx(1),1:mx(2)),                &
!                     vhe(1:mx(1),0:mx(2)))                  
! 
! ! updateresult(1:mx(1),1:mx(2),qf2) =                      &
! !     - dt_2*(UWeno2D(qnew(-1:mx(1)+2,-1:mx(2)+2,qf2),       &
! !                     uhe(0:mx(1),1:mx(2)),             &
! !                     vhe(1:mx(1),0:mx(2)))/h             &
! !     -      NGradUN2D(Normal2D(qnew(0:mx(1)+1,0:mx(2)+1,qc),h),&
! !                      qnew(0:mx(1)+1,0:mx(2)+1,qu), &
! !                      qnew(0:mx(1)+1,0:mx(2)+1,qv),h)&
! !                     *qnew(-1:mx(1)+2,-1:mx(2)+2,qf2))&
! !   + 0.5*dt*surfdiff*LapSurf2D(qnew(0:mx(1)+1,0:mx(2)+1,qc),  &
! !                    qnew(0:mx(1)+1,0:mx(2)+1,qf2),         &
! !                    xlower,h)
! endif


end function SourceUpdate2D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
pure function NeuBC(c,h) result(neubcresult)
! Calculation of |grad(c)|
use NodeInfoDef
use ProblemDef
implicit none
real, dimension(0:,0:), intent(in) :: c
real, intent(in) :: h
integer, dimension(2) :: mx
real, dimension(size(c,1)-2, size(c,2)-2) :: c_x, c_y, neubcresult

mx = (/ size(c,1)-2, size(c,2)-2 /)

c_x = c(2:mx(1)+1,1:mx(2))-c(0:mx(1)-1,1:mx(2))
c_y = c(1:mx(1),2:mx(2)+1)-c(1:mx(1),0:mx(2)-1)

neubcresult = 0.5*sqrt(c_x*c_x + c_y*c_y)/h

end function NeuBC

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
function Lap2D(a,xlower,h) result(lapresult)
use NodeInfoDef
use ProblemDef
use GridUtilities, only: ULap2D
implicit none
!
! Level independent, 2D laplacian operator 
! e.g nabla^2(a). Also works in axisymmetry
!
real(kind=r8), dimension(0:,0:), intent(in):: a 
real, dimension(2), intent(in) :: xlower
real, intent(in) :: h
real(kind=r8), dimension(1:size(a,1)-2,1:size(a,2)-2):: lapresult
!
integer, dimension(1:2):: mx
real,dimension(1:size(a,1)-2,1:size(a,2)-2) :: xs
!
mx(1) = size(a,1)-2; mx(2) = size(a,2)-2

lapresult = ULap2D(a)/(h*h)

if (axisym) then
  xs = GetX(xlower,mx,h)
  lapresult = lapresult + (a(2:mx(1)+1,1:mx(2))-a(0:mx(1)-1,1:mx(2)))/(2.0*xs*h)
endif

! Old loopy version 
! if (axisym) then                                      
!   do j = 1,mx(2)
!     do i = 1,mx(1)
!       if(j==1) x(i) = xlower(1) + (real(i)-0.5)*h
!       lapresult(i,j) = lapresult(i,j) + (a(i+1,j)-a(i-1,j))/(2.0*x(i)*h)
!     enddo
!   enddo
! endif

end function Lap2D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
function CDiv2D(f,xlower,h) result(divresult)
! Gives cell centered divergence from cell centered vectors
use NodeInfoDef
use ProblemDef
implicit none
!
real, dimension(0:,0:,1:), intent(in):: f
real, dimension(2),intent(in) :: xlower
real, intent(in) :: h
real, dimension(1:size(f,1)-2,1:size(f,2)-2) :: divresult
!
integer, dimension(1:2):: mx
real, dimension(1:size(f,1)-2,1:size(f,2)-2) :: xs
!
mx(1) = size(f,1)-2; mx(2) = size(f,2)-2

divresult = 0.5*(f(2:mx(1)+1,1:mx(2),1)-f(0:mx(1)-1,1:mx(2),1) &
          +      f(1:mx(1),2:mx(2)+1,2)-f(1:mx(1),0:mx(2)-1,2))/h 

if(axisym) then
  ! NB this is cell centered...
  xs = GetX(xlower,mx,h)
  divresult = divresult + f(1:mx(1),1:mx(2),1)/xs
endif

end function CDiv2D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
function Div2D(f,xlower,h) result(divresult)
! Gives cell centered divergence from edge centered vectors
use NodeInfoDef
use ProblemDef
implicit none
!
real, dimension(0:,0:,1:), intent(in):: f
real, dimension(2),intent(in) :: xlower
real, intent(in) :: h
real, dimension(1:size(f,1)-1,1:size(f,2)-1) :: divresult
!
integer, dimension(1:2):: mx
real, dimension(1:size(f,1)-1,1:size(f,2)-1) :: xs
!
mx(1) = size(f,1)-1; mx(2) = size(f,2)-1

divresult = (f(1:mx(1),1:mx(2),1)-f(0:mx(1)-1,1:mx(2),1) &
           + f(1:mx(1),1:mx(2),2)-f(1:mx(1),0:mx(2)-1,2))/h 

if(axisym) then
  ! NB this is cell centered...
  xs = GetX(xlower,mx,h)
  divresult = divresult + 0.5*(f(1:mx(1),1:mx(2),1)+f(0:mx(1)-1,1:mx(2),1))/xs
endif

end function Div2D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
! function DivRhieChow2D(u,p,rhos,xlower,h) result(divresult)
function DivRhieChow2D(u,p,xlower,h) result(divresult)
! Rhie-Chow interpolation
use NodeInfoDef
use ProblemDef
implicit none
!
real, dimension(-1:,-1:,1:), intent(in):: u
real, dimension(-1:,-1:), intent(in):: p
! real, dimension(-1:,-1:), intent(in):: rhos
real, dimension(0:size(u,1)-2,0:size(u,2)-2) :: rhos 
real, dimension(2),intent(in) :: xlower
real, intent(in) :: h
real, dimension(1:size(u,1)-3,1:size(u,2)-3):: divresult
real, dimension(0:size(u,1)-2,0:size(u,2)-2,2):: ut
real, dimension(0:size(u,1)-3,0:size(u,2)-3,2):: us
!
integer, dimension(1:2):: mx
real :: a,tmp
!
! TODO - According to Armfield, the correction is not needed in the pressure
! correction method...but this removes oscillations...

mx(1) = size(u,1)-3; mx(2) = size(u,2)-3
tmp = 0.5*dt/h
tmp = 0.0
rhos = 1.0

ut(0:mx(1)+1,0:mx(2)+1,1) = u(0:mx(1)+1,0:mx(2)+1,1) &
  + tmp*rhos(0:mx(1)+1,0:mx(2)+1) &
  * (p(1:mx(1)+2,0:mx(2)+1)-p(-1:mx(1),0:mx(2)+1))

ut(0:mx(1)+1,0:mx(2)+1,2) = u(0:mx(1)+1,0:mx(2)+1,2) &
  + tmp*rhos(0:mx(1)+1,0:mx(2)+1) &
  * (p(0:mx(1)+1,1:mx(2)+2)-p(0:mx(1)+1,-1:mx(2)))

us(0:mx(1),1:mx(2),1) = 0.5*(ut(1:mx(1)+1,1:mx(2),1) + ut(0:mx(1),1:mx(2),1)) &
  - tmp*(rhos(0:mx(1),1:mx(2))+rhos(1:mx(1)+1,1:mx(2))) &
  * (p(1:mx(1)+1,1:mx(2))-p(0:mx(1),1:mx(2)))

us(1:mx(1),0:mx(2),2) = 0.5*(ut(1:mx(1),1:mx(2)+1,2) + ut(1:mx(1),0:mx(2),2)) &
  - tmp*(rhos(1:mx(1),0:mx(2))+rhos(1:mx(1),1:mx(2)+1)) &
  * (p(1:mx(1),1:mx(2)+1)-p(1:mx(1),0:mx(2)))

divresult(1:mx(1),1:mx(2)) = Div2D(us(0:mx(1),0:mx(2),1:2),xlower,h)

end function DivRhieChow2D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
function Convec2D(f,u,h) result(convec)
use NodeInfoDef
use ProblemDef
implicit none
!
real, dimension(0:,0:,1:), intent(in):: f
real, dimension(0:,0:,1:), intent(in):: u
real, intent(in) :: h
real, dimension(0:size(u,1)-2,1:size(u,2)-2,2):: uedgex
real, dimension(1:size(u,1)-2,0:size(u,2)-2,2):: uedgey

real, dimension(1:size(u,1)-2,1:size(u,2)-2,2):: convec
!
integer, dimension(1:2):: mx
integer :: i
real :: a,tmp
!
mx(1) = size(u,1)-2; mx(2) = size(u,2)-2
! 
uedgex(0:mx(1),1:mx(2),1:2) = u(0:mx(1)  ,1:mx(2),1:2)  &
                            + u(1:mx(1)+1,1:mx(2),1:2)

uedgey(1:mx(1),0:mx(2),1:2) = u(1:mx(1),0:mx(2)  ,1:2)  &
                            + u(1:mx(1),1:mx(2)+1,1:2)


do i=1,2
convec(1:mx(1),1:mx(2),i) &
  = f(1:mx(1)  ,1:mx(2)  ,1)*uedgex(1:mx(1)  ,1:mx(2)  ,i) &
  - f(0:mx(1)-1,1:mx(2)  ,1)*uedgex(0:mx(1)-1,1:mx(2)  ,i) &
  + f(1:mx(1)  ,1:mx(2)  ,2)*uedgey(1:mx(1)  ,1:mx(2)  ,i) &
  - f(1:mx(1)  ,0:mx(2)-1,2)*uedgey(1:mx(1)  ,0:mx(2)-1,i)
enddo
  
convec=0.5*convec/h

end function Convec2D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
function EGrad2D(a,h) result(gradresult)
use NodeInfoDef
use ProblemDef
implicit none
!
! Takes the gradient of a cell centered variable. The output is
! the gradient located at the edges.
!
real(kind=r8), dimension(0:,0:), intent(in):: a 
real, intent(in) :: h
real(kind=r8), dimension(0:size(a,1)-2,0:size(a,2)-2,2):: gradresult
!
integer, dimension(1:2):: mx
integer :: i,j
!
mx(1) = size(a,1)-2; mx(2) = size(a,2)-2

gradresult = 0.0

gradresult(0:mx(1),1:mx(2),1) = a(1:mx(1)+1,1:mx(2)) &
                              - a(0:mx(1),  1:mx(2))

gradresult(1:mx(1),0:mx(2),2) = a(1:mx(1),1:mx(2)+1) &
                              - a(1:mx(1),0:mx(2)  )

gradresult = gradresult/h

end function EGrad2D

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
function Grad2D(a,h) result(gradresult)
! Takes the gradient of a cell centered variable. The output is
! the gradient located at the cell centers.
use NodeInfoDef
use ProblemDef
use GridUtilities, only: ULap2D
implicit none
!
real(kind=r8), dimension(0:,0:), intent(in):: a 
real, intent(in) :: h
real(kind=r8), dimension(1:size(a,1)-2,1:size(a,2)-2,2):: gradresult
!
integer, dimension(1:2):: mx
integer :: i,j
!
mx(1) = size(a,1)-2; mx(2) = size(a,2)-2

gradresult(:,:,1) = a(2:mx(1)+1,1:mx(2))-a(0:mx(1)-1,1:mx(2)) 
gradresult(:,:,2) = a(1:mx(1),2:mx(2)+1)-a(1:mx(1),0:mx(2)-1) 

gradresult = 0.5*gradresult/h

end function Grad2D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
function LapVar2D(c,a,xlower,h) result(lapvarresult)
use NodeInfoDef
use ProblemDef
implicit none
!
! Level independent, 2D laplacian operator for non-constant 
! diffusivity, e.g div*(c*grad(a)).
!
real(kind=r8), dimension(0:,0:), intent(in):: c 
real(kind=r8), dimension(0:,0:), intent(in):: a
real, dimension(2), intent(in) :: xlower
real, intent(in) :: h
real(kind=r8), dimension(1:size(a,1)-2,1:size(a,2)-2):: lapvarresult
!
integer, dimension(1:2):: mx
real, dimension(1:size(a,1)-2,1:size(a,2)-2):: xs
real, dimension(1:size(a,1)-2) :: x
integer :: i,j
!
mx(1) = size(a,1)-2; mx(2) = size(a,2)-2

lapvarresult = ULapVar2D(c,a)/(h*h)

if (axisym) then
  xs = GetX(xlower,mx,h)
  lapvarresult = lapvarresult + c(1:mx(1),1:mx(2)) &
               * (a(2:mx(1)+1,1:mx(2))-a(0:mx(1)-1,1:mx(2)))/(2.0*xs*h)
endif

! Old loopy version
! if(axisym) then                                      
!   do j = 1,mx(2)
!     do i = 1,mx(1)
!       if(j==1) x(i) = xlower(1) + (real(i)-0.5)*h
!       lapvarresult(i,j) = lapvarresult(i,j) + &
!                           c(i,j)*(a(i+1,j)-a(i-1,j))/(2.0*x(i)*h)
!     enddo
!   enddo
! endif

end function LapVar2D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
function LapSurf2D(c,f,xlower,h) result(lapresult)
use NodeInfoDef
use ProblemDef
use GridUtilities, only: UDiv2D
implicit none
!
! Level independent, 2D laplacian operator for non-constant 
! diffusivity, e.g div*(c*grad(a)).
!
real(kind=r8), dimension(0:,0:), intent(in):: c 
real(kind=r8), dimension(0:,0:), intent(in):: f
real, dimension(2), intent(in) :: xlower
real, intent(in) :: h
real(kind=r8), dimension(1:size(c,1)-2,1:size(c,2)-2):: lapresult
integer, dimension(1:2):: mx
real, dimension(1:size(c,1)-2,1:size(c,2)-2):: c_xx,c_yy,c_xy,c_x,c_y,denom
real, dimension(1:size(c,1)-2,1:size(c,2)-2):: f_xx,f_yy,f_xy,f_x,f_y
real, dimension(1:size(c,1)-2,1:size(c,2)-2):: curv,delta
real :: invh,invh2
real, parameter :: small=1e-6
real, dimension(1:size(c,1)-2,1:size(c,2)-2,2) :: n

mx(1) = size(c,1)-2; mx(2) = size(c,2)-2

invh = 1.0/h
invh2 = invh*invh

! c_xx = (c(2:mx(1)+1,1:mx(2))+c(0:mx(1)-1,1:mx(2))-2.0*c(1:mx(1),1:mx(2)))*invh2
! c_yy = (c(1:mx(1),2:mx(2)+1)+c(1:mx(1),0:mx(2)-1)-2.0*c(1:mx(1),1:mx(2)))*invh2

c_x = 0.5*(c(2:mx(1)+1,1:mx(2))-c(0:mx(1)-1,1:mx(2)))*invh
c_y = 0.5*(c(1:mx(1),2:mx(2)+1)-c(1:mx(1),0:mx(2)-1))*invh

! c_xy = 0.25*(c(2:mx(1)+1,2:mx(2)+1) - c(2:mx(1)+1,0:mx(2)-1) &
!      +       c(0:mx(1)-1,0:mx(2)-1) - c(0:mx(1)-1,2:mx(2)+1))*invh2
! 
! denom = (c_x**2 + c_y**2)**1.5

curv = CurvatureKim2D(c,h)

! The convention is negative curvature
! where(abs(denom) > small)
!   curv = -(c_x**2*c_yy - 2.0*c_x*c_y*c_xy + c_y**2*c_xx)/denom
! end where

delta = sqrt(c_x**2 + c_y**2)

n = 0.0
where(abs(delta) > small)
  n(:,:,1) = c_x/delta
  n(:,:,2) = c_y/delta
end where

f_xx = (f(2:mx(1)+1,1:mx(2))+f(0:mx(1)-1,1:mx(2))-2.0*f(1:mx(1),1:mx(2)))*invh2
f_yy = (f(1:mx(1),2:mx(2)+1)+f(1:mx(1),0:mx(2)-1)-2.0*f(1:mx(1),1:mx(2)))*invh2

f_x = 0.5*(f(2:mx(1)+1,1:mx(2))-f(0:mx(1)-1,1:mx(2)))*invh
f_y = 0.5*(f(1:mx(1),2:mx(2)+1)-f(1:mx(1),0:mx(2)-1))*invh

f_xy = 0.25*(f(2:mx(1)+1,2:mx(2)+1) - f(2:mx(1)+1,0:mx(2)-1) &
     +       f(0:mx(1)-1,0:mx(2)-1) - f(0:mx(1)-1,2:mx(2)+1))*invh2

lapresult = (f_xx + f_yy) &
          - (n(:,:,1)**2*f_xx + 2.0*n(:,:,1)*n(:,:,2)*f_xy + n(:,:,2)**2*f_yy) &
          + curv*(n(:,:,1)*f_x + n(:,:,2)*f_y)
          !TODO pluss eller minus i siste ledd? pluss!

end function LapSurf2D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
function GetY(xlower,mx,h) result(ys)
use NodeInfoDef
implicit none
real, dimension(2), intent(in) :: xlower
integer, dimension(2), intent(in) :: mx
real, intent(in) :: h
!
real, dimension(mx(2)) :: y
real, dimension(mx(1),mx(2)) :: ys
integer :: j

do j = 1,mx(2)
  y(j) = xlower(2) + (real(j)-0.5)*h
enddo
ys = spread(y,1,mx(1))

end function GetY
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
function GetX(xlower,mx,h) result(xs)
use NodeInfoDef
implicit none
real, dimension(2), intent(in) :: xlower
integer, dimension(2), intent(in) :: mx
real, intent(in) :: h
!
real, dimension(mx(1)) :: x
real, dimension(mx(1),mx(2)) :: xs
integer :: i

do i = 1,mx(1)
  x(i) = xlower(1) + (real(i)-0.5)*h
enddo
xs = spread(x,2,mx(2))

end function GetX
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
function ULapVar2D(c,a) result(lapvarresult)
use NodeInfoDef
use GridUtilities, only: UDiv2D
implicit none
!
! Level independent, 2D UNDIVIDED laplacian operator for non-constant 
! diffusivity, e.g div*(c*grad(a)).
!
real(kind=r8), dimension(0:,0:), intent(in):: c 
real(kind=r8), dimension(0:,0:), intent(in):: a
real(kind=r8), dimension(1:size(a,1)-2,1:size(a,2)-2):: lapvarresult
!
integer, dimension(1:2):: mx
real(kind=r8), dimension(0:size(a,1)-2  ,1:size(a,2)-2  ):: f1
real(kind=r8), dimension(1:size(a,1)-2  ,0:size(a,2)-2  ):: f2
real(kind=r8), dimension(0:size(a,1)-2+1,0:size(a,2)-2+1):: m
!
mx(1) = size(a,1)-2; mx(2) = size(a,2)-2
!
m(0:mx(1)+1,0:mx(2)+1) = 0.5*(c(0:mx(1)+1,0:mx(2)+1))
!
! Calculate the UNDIVIDED 2D flux function:
f1(0:mx(1),1:mx(2)) &
  = (m(1:mx(1)+1,1:mx(2))+m(0:mx(1),1:mx(2))) &
  * (a(1:mx(1)+1,1:mx(2))-a(0:mx(1),1:mx(2)))
!
f2(1:mx(1),0:mx(2)) &
  = (m(1:mx(1),1:mx(2)+1)+m(1:mx(1),0:mx(2))) &
  * (a(1:mx(1),1:mx(2)+1)-a(1:mx(1),0:mx(2)))
!
! Calculate the UNDIVIDED divergence of the flux:
lapvarresult(1:mx(1),1:mx(2)) = UDiv2D(f1(0:mx(1),1:mx(2)), &
                                       f2(1:mx(1),0:mx(2)))

end function ULapVar2D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
function UEno2D(c,u1,u2) result(uenoresult)
use NodeInfoDef
implicit none
!
real, dimension(-1:,-1:), intent(in):: c
real, dimension(0:,1:), intent(in):: u1
real, dimension(1:,0:), intent(in):: u2
real, dimension(1:size(c,1)-4, &
                         1:size(c,2)-4):: uenoresult
!
integer, dimension(1:2):: mx
real, dimension(0:size(c,1)-4, &
                         1:size(c,2)-4):: f1
real, dimension(1:size(c,1)-4, &
                         0:size(c,2)-4):: f2
!
integer:: i, i1, j, j1
real:: aa, bb, cc
!
mx(1) = size(c,1)-4; mx(2) = size(c,2)-4
!
do i = 0, mx(1)
  do j = 1, mx(2)
    if(u1(i,j)>0.0) then
      i1 = i
    else
      i1 = i+1
    end if
    aa = c(i1,j)-c(i1-1,j)
    bb = c(i1+1,j)-c(i1,j)
    if(abs(aa)<=abs(bb)) then
      cc = aa
    else
      cc = bb
    end if
    f1(i,j) = (c(i1,j)+cc*(1.0-2.0*real(i1-i,kind=r8))/2.0) &
            * u1(i,j)
  end do
end do
!
do i = 1, mx(1)
  do j = 0, mx(2)
    if(u2(i,j)>0.0) then
      j1 = j
    else
      j1 = j+1
    end if
    aa = c(i,j1)-c(i,j1-1)
    bb = c(i,j1+1)-c(i,j1)
    if(abs(aa)<=abs(bb)) then
      cc = aa
    else
      cc = bb
    end if
    f2(i,j) = (c(i,j1)+cc*(1.0-2.0*real(j1-j,kind=r8))/2.0) &
              * u2(i,j)
  end do
end do
!
uenoresult(1:mx(1),1:mx(2)) &
  =  f1(1:mx(1),1:mx(2))-f1(0:mx(1)-1,1:mx(2)  ) &
  +  f2(1:mx(1),1:mx(2))-f2(1:mx(1)  ,0:mx(2)-1)
!
end function UEno2D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
function UWeno2D(c,u,v) result(uwenoresult)
use NodeInfoDef
implicit none
!
! 3rd-order WENO reconstruction.
! uvec\dot\grad c
!
real, dimension(-1:,-1:), intent(in):: c
real, dimension(0:,1:):: u
real, dimension(1:,0:):: v
real, dimension(1:size(c,1)-4,1:size(c,2)-4):: uwenoresult
!
integer:: i, j
integer, dimension(1:2):: mx
real:: a11, a12, a21, a22, b1, b2, &
                g1, g2, v1, v2, w1, w2, f1, f2
real, dimension(0:size(c,1)-4,1:size(c,2)-4):: few
real, dimension(1:size(c,1)-4,0:size(c,2)-4):: fns
!
a11 = -1.0/2.0; a12 = 3.0/2.0
a21 =  1.0/2.0; a22 = 1.0/2.0
!
g1 = 1.0/3.0
g2 = 2.0/3.0
!
mx(1) = size(c,1)-4; mx(2) = size(c,2)-4
!
do i = 0, mx(1)
  do j = 1, mx(2)
    if(u(i,j)>0.0) then
      f1 = a11*c(i-1,j)+a12*c(i  ,j)
      f2 = a21*c(i  ,j)+a22*c(i+1,j)
      b1 = Sqr(c(i  ,j)-c(i-1,j))
      b2 = Sqr(c(i+1,j)-c(i  ,j))
      v1 = g1/Sqr(1.0E-06+b1); v2 = g2/Sqr(1.0E-06+b2)
      w1 = v1/(v1+v2); w2 = v2/(v1+v2)
    else
      f1 = a11*c(i+2,j)+a12*c(i+1,j)
      f2 = a21*c(i+1,j)+a22*c(i  ,j)
      b1 = Sqr(c(i+2,j)-c(i+1,j))
      b2 = Sqr(c(i+1,j)-c(i  ,j))
      v1 = g1/Sqr(1.0E-06+b1); v2 = g2/Sqr(1.0E-06+b2)
      w1 = v1/(v1+v2); w2 = v2/(v1+v2)
    end if
    few(i,j) = (f1*w1+f2*w2)*u(i,j)
  end do
end do
!
do i = 1, mx(1)
  do j = 0, mx(2)
    if(v(i,j)>0.0) then
      f1 = a11*c(i,j-1)+a12*c(i,j  )
      f2 = a21*c(i,j  )+a22*c(i,j+1)
      b1 = Sqr(c(i,j  )-c(i,j-1))
      b2 = Sqr(c(i,j+1)-c(i,j  ))
      v1 = g1/Sqr(1.0E-06+b1); v2 = g2/Sqr(1.0E-06+b2)
      w1 = v1/(v1+v2); w2 = v2/(v1+v2)
    else
      f1 = a11*c(i,j+2)+a12*c(i,j+1)
      f2 = a21*c(i,j+1)+a22*c(i,j  )
      b1 = Sqr(c(i,j+2)-c(i,j+1))
      b2 = Sqr(c(i,j+1)-c(i,j  ))
      v1 = g1/Sqr(1.0E-06+b1); v2 = g2/Sqr(1.0E-06+b2)
      w1 = v1/(v1+v2); w2 = v2/(v1+v2)
    end if
    fns(i,j) = (f1*w1+f2*w2)*v(i,j)
  end do
end do
!
uwenoresult(1:mx(1),1:mx(2)) &
  = (few(1:mx(1),1:mx(2))-few(0:mx(1)-1,1:mx(2)  ) &
  +  fns(1:mx(1),1:mx(2))-fns(1:mx(1)  ,0:mx(2)-1))
!!!!
end function UWeno2D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!function SurfTen2D2(c,chem,xlower,h) result(surftenresult)
!use NodeInfoDef
!use ProblemDef
!implicit none
!real, dimension(1:,1:), intent(in):: c
!real, dimension(0:,0:), intent(in):: chem
!real, dimension(2), intent(in) :: xlower
!real, intent(in) :: h
!
!integer, dimension(2) :: mx
!real, dimension(1:size(c,1),1:size(c,2),2):: surftenresult
!real, dimension(1:size(c,1),1:size(c,2)):: chem_x,chem_y
!real :: invh,invh2
!real, parameter :: small=1e-6
!
!mx(1) = size(c,1); mx(2) = size(c,2)
!
!invh = 1.0/h
!invh2 = invh*invh
!
!chem_x = 0.5*(chem(2:mx(1)+1,1:mx(2))-chem(0:mx(1)-1,1:mx(2)))*invh
!chem_y = 0.5*(chem(1:mx(1),2:mx(2)+1)-chem(1:mx(1),0:mx(2)-1))*invh
!
!surftenresult(:,:,1) = -sigma0*alpha*c*chem_x/eps
!surftenresult(:,:,2) = -sigma0*alpha*c*chem_y/eps
!
!end function SurfTen2D2
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
function Cap2D(c,sigma,xlower,h) result(capresult)
use NodeInfoDef
use ProblemDef
implicit none
real, dimension(0:,0:), intent(in):: c,sigma
real, dimension(2), intent(in) :: xlower
real, intent(in) :: h
!
integer, dimension(2) :: mx
real, dimension(1:size(c,1)-2,1:size(c,2)-2):: capresult
mx(1) = size(c,1)-2; mx(2) = size(c,2)-2

capresult = -sigma(1:mx(1),1:mx(2))*CurvatureKim2D(c,h);

end function Cap2D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
function MarVec2D(c,sigma,xlower,h) result(marresult)
use NodeInfoDef
use ProblemDef
implicit none
real, dimension(0:,0:), intent(in):: c,sigma
real, dimension(2), intent(in) :: xlower
real, intent(in) :: h
!
integer, dimension(2) :: mx
real, dimension(1:size(c,1)-2,1:size(c,2)-2,2):: surftenresult
real, dimension(1:size(c,1)-2,1:size(c,2)-2,2):: marresult
real, dimension(1:size(c,1)-2,1:size(c,2)-2):: c_xx,c_yy,c_xy,c_x,c_y,denom
real, dimension(1:size(c,1)-2,1:size(c,2)-2):: curv,delta,xs,sigma_x,sigma_y
real :: invh,invh2
real, parameter :: small=1e-6
real, dimension(1:size(c,1)-2,1:size(c,2)-2,2) :: n,t
mx(1) = size(c,1)-2; mx(2) = size(c,2)-2

invh = 1.0/h
invh2 = invh*invh

c_x = 0.5*(c(2:mx(1)+1,1:mx(2))-c(0:mx(1)-1,1:mx(2)))*invh
c_y = 0.5*(c(1:mx(1),2:mx(2)+1)-c(1:mx(1),0:mx(2)-1))*invh

delta = sqrt(c_x**2 + c_y**2)

sigma_x = 0.5*(sigma(2:mx(1)+1,1:mx(2)) - sigma(0:mx(1)-1,1:mx(2)))*invh
sigma_y = 0.5*(sigma(1:mx(1),2:mx(2)+1) - sigma(1:mx(1),0:mx(2)-1))*invh

n = 0.0
where(abs(delta) > small)
  n(:,:,1) = -c_x/delta
  n(:,:,2) = -c_y/delta
end where
t(:,:,1) = -n(:,:,2)
t(:,:,2) =  n(:,:,1)

! grad_s = (I - nn)grad:
marresult(:,:,1) = &
 + (n(:,:,2)**2*sigma_x - n(:,:,1)*n(:,:,2)*sigma_y)*delta
 
marresult(:,:,2) = &
 + (n(:,:,1)**2*sigma_y - n(:,:,1)*n(:,:,2)*sigma_x)*delta

! marresult(:,:)=&
!  ((n(:,:,2)**2)*sigma_x - n(:,:,1)*n(:,:,2)*sigma_y)*delta*t(:,:,1) &
! +((n(:,:,1)**2)*sigma_y - n(:,:,1)*n(:,:,2)*sigma_x)*delta*t(:,:,2)

! tangent version
! marresult(:,:,1) = ((sigma_x*t(:,:,1) &
!                  +   sigma_y*t(:,:,2)))*t(:,:,1)*delta
! 
! marresult(:,:,2) = ((sigma_x*t(:,:,1) &
!                  +   sigma_y*t(:,:,2)))*t(:,:,2)*delta

end function MarVec2D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
function Mar2D(c,sigma,xlower,h) result(marresult)
use NodeInfoDef
use ProblemDef
implicit none
real, dimension(0:,0:), intent(in):: c,sigma
real, dimension(2), intent(in) :: xlower
real, intent(in) :: h
!
integer, dimension(2) :: mx
real, dimension(1:size(c,1)-2,1:size(c,2)-2,2):: surftenresult
real, dimension(1:size(c,1)-2,1:size(c,2)-2):: marresult
real, dimension(1:size(c,1)-2,1:size(c,2)-2):: c_x,c_y
real, dimension(1:size(c,1)-2,1:size(c,2)-2):: delta,sigma_x,sigma_y
real :: invh
real, parameter :: small=1e-6
real, dimension(1:size(c,1)-2,1:size(c,2)-2,2) :: n,t

mx(1) = size(c,1)-2; mx(2) = size(c,2)-2

invh = 1.0/h

c_x = 0.5*(c(2:mx(1)+1,1:mx(2))-c(0:mx(1)-1,1:mx(2)))*invh
c_y = 0.5*(c(1:mx(1),2:mx(2)+1)-c(1:mx(1),0:mx(2)-1))*invh

delta = sqrt(c_x**2 + c_y**2)

sigma_x = 0.5*(sigma(2:mx(1)+1,1:mx(2)) - sigma(0:mx(1)-1,1:mx(2)))*invh
sigma_y = 0.5*(sigma(1:mx(1),2:mx(2)+1) - sigma(1:mx(1),0:mx(2)-1))*invh

! Alternative delta function:
! delta = 3.0*sqrt(2.0)*c(1:mx(1),1:mx(2))**2*(1.0-c(1:mx(1),1:mx(2)))**2

n = 0.0
where(abs(delta) > small)
  n(:,:,1) = -c_x/delta
  n(:,:,2) = -c_y/delta
end where

t(:,:,1) = -n(:,:,2)
t(:,:,2) =  n(:,:,1)

! grad_s = (I - nn)grad:
marresult(:,:)=&
 ((n(:,:,2)**2)*sigma_x - n(:,:,1)*n(:,:,2)*sigma_y)*t(:,:,1) &
+((n(:,:,1)**2)*sigma_y - n(:,:,1)*n(:,:,2)*sigma_x)*t(:,:,2)

end function Mar2D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
function SurfTen2D(c,sigma,xlower,h) result(surftenresult)
use NodeInfoDef
use ProblemDef
implicit none
real, dimension(0:,0:), intent(in):: c,sigma
real, dimension(2), intent(in) :: xlower
real, intent(in) :: h
!
integer, dimension(2) :: mx
real, dimension(1:size(c,1)-2,1:size(c,2)-2,2):: surftenresult
real, dimension(1:size(c,1)-2,1:size(c,2)-2):: c_xx,c_yy,c_xy,c_x,c_y,denom
real, dimension(1:size(c,1)-2,1:size(c,2)-2):: curv,delta,xs,sigma_x,sigma_y
real :: invh,invh2
real, parameter :: small=1e-9
real, dimension(1:size(c,1)-2,1:size(c,2)-2,2) :: n,t
mx(1) = size(c,1)-2; mx(2) = size(c,2)-2

invh = 1.0/h
invh2 = invh*invh

c_x = 0.5*(c(2:mx(1)+1,1:mx(2))-c(0:mx(1)-1,1:mx(2)))*invh
c_y = 0.5*(c(1:mx(1),2:mx(2)+1)-c(1:mx(1),0:mx(2)-1))*invh

delta = sqrt(c_x**2 + c_y**2)

sigma_x = 0.5*(sigma(2:mx(1)+1,1:mx(2)) - sigma(0:mx(1)-1,1:mx(2)))*invh
sigma_y = 0.5*(sigma(1:mx(1),2:mx(2)+1) - sigma(1:mx(1),0:mx(2)-1))*invh

n = 0.0
where(abs(delta) > small)
n(:,:,1) = -c_x/delta
n(:,:,2) = -c_y/delta
end where
! t(:,:,1) = -n(:,:,2)
! t(:,:,2) =  n(:,:,1)

curv = CurvatureKim2D(c,h)

! Alternative grad_s = (I - nn)grad:
! Remember: n1^2 + n2^2 = 1
surftenresult(:,:,1) = (sigma(1:mx(1),1:mx(2))*curv*c_x &
 + (n(:,:,2)**2*sigma_x - n(:,:,1)*n(:,:,2)*sigma_y)*delta)

surftenresult(:,:,2) = (sigma(1:mx(1),1:mx(2))*curv*c_y &
 + (n(:,:,1)**2*sigma_y - n(:,:,1)*n(:,:,2)*sigma_x)*delta)

! grad_s = grad\dot t
! surftenresult(:,:,1) = -sigma(1:mx(1),1:mx(2))*curv*c_x &
!  + (t(:,:,1)*sigma_x + t(:,:,2)*sigma_y)*t(:,:,1)*delta
! 
! surftenresult(:,:,2) = -sigma(1:mx(1),1:mx(2))*curv*c_y &
!  + (t(:,:,1)*sigma_x + t(:,:,2)*sigma_y)*t(:,:,2)*delta

! sigma*kappa*delta*n
! surftenresult(:,:,1) = sigma(1:mx(1),1:mx(2))*curv*n(:,:,1)*delta 
!  + ((1.0-n(:,:,1)**2)*sigma_x - n(:,:,1)*n(:,:,2)*sigma_y)*delta
! 
! surftenresult(:,:,2) = sigma(1:mx(1),1:mx(2))*curv*n(:,:,2)*delta 
!  + ((1.0-n(:,:,2)**2)*sigma_y - n(:,:,1)*n(:,:,2)*sigma_x)*delta

end function SurfTen2D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
function Normal2D(c,h) result(normal)
use NodeInfoDef
use ProblemDef
implicit none
real, dimension(0:,0:), intent(in):: c
real, intent(in) :: h
!
integer, dimension(2) :: mx
real, dimension(1:size(c,1)-2,1:size(c,2)-2,2):: normal
real, dimension(1:size(c,1)-2,1:size(c,2)-2,2):: gradc
real, dimension(1:size(c,1)-2,1:size(c,2)-2):: denom
real :: invh
real, parameter :: small=1e-6

mx(1) = size(c,1)-2; mx(2) = size(c,2)-2

invh = 1.0/h

gradc(:,:,1) = 0.5*(c(2:mx(1)+1,1:mx(2))-c(0:mx(1)-1,1:mx(2)))*invh
gradc(:,:,2) = 0.5*(c(1:mx(1),2:mx(2)+1)-c(1:mx(1),0:mx(2)-1))*invh

denom = sqrt(gradc(:,:,1)**2 + gradc(:,:,2)**2)

where(abs(denom) > small)
  normal(:,:,1) = -gradc(:,:,1)/denom
  normal(:,:,2) = -gradc(:,:,2)/denom
elsewhere
  normal(:,:,1) = 0.0
  normal(:,:,2) = 0.0
end where

end function Normal2D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
function NGradUN2D(n,u,v,h) result(ngradun)
! Undivided calculation of \vec n = gradc/|grad(c)|
! NB! Center velocities...
use NodeInfoDef
use ProblemDef
implicit none
real, dimension(1:,1:,1:), intent(in) :: n
real, dimension(0:,0:),intent (in):: u
real, dimension(0:,0:),intent (in):: v
real, intent(in) :: h
integer, dimension(2) :: mx
real, dimension(size(n,1), size(n,2)) :: u_x, u_y, v_x, v_y, ngradun

mx = (/ size(n,1), size(n,2) /)

u_x = 0.5*(u(2:mx(1)+1,1:mx(2))-u(0:mx(1)-1,1:mx(2)))/h
v_x = 0.5*(v(2:mx(1)+1,1:mx(2))-v(0:mx(1)-1,1:mx(2)))/h

u_y = 0.5*(u(1:mx(1),2:mx(2)+1)-u(1:mx(1),0:mx(2)-1))/h
v_y = 0.5*(v(1:mx(1),2:mx(2)+1)-v(1:mx(1),0:mx(2)-1))/h

ngradun = u_x*n(:,:,1)**2 &
        + n(:,:,1)*n(:,:,2)*(u_y+v_x) &
        + v_y*n(:,:,2)**2

end function NGradUN2D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
function CurvatureKim2D(c,h) result(curv)
use NodeInfoDef
use ProblemDef
implicit none
real, dimension(0:,0:), intent(in):: c
real, intent(in) :: h
!
integer, dimension(2) :: mx
real, dimension(1:size(c,1)-2,1:size(c,2)-2):: curv
real :: invh,invh2
real :: nppx,nppy,lnpp,npmx,npmy,lnpm,nmpx,nmpy,lnmp,nmmx,nmmy,lnmm
real, dimension(0:size(c,1)-2,0:size(c,2)-2,2) :: n
real, dimension(0:size(c,1)-2,0:size(c,2)-2) :: ln
real, parameter :: small=1e-6
integer :: i,j

mx(1) = size(c,1)-2; mx(2) = size(c,2)-2


n(:,:,1) = (c(1:mx(1)+1,0:mx(2)) + c(1:mx(1)+1,1:mx(2)+1) &
         -  c(0:mx(1)  ,0:mx(2)) - c(0:mx(1)  ,1:mx(2)+1))

n(:,:,2) = (c(0:mx(1),1:mx(2)+1) + c(1:mx(1)+1,1:mx(2)+1) &
         -  c(0:mx(1),0:mx(2)  ) - c(1:mx(1)+1,0:mx(2)  ))

! n = 0.5*n/h ! This will get divided away below

ln = sqrt(n(:,:,1)**2 + n(:,:,2)**2) + small

curv=(n(1:mx(1)  ,1:mx(2)  ,1)+n(1:mx(1)  ,1:mx(2)  ,2))/ln(1:mx(1)  ,1:mx(2)  )&
    +(n(1:mx(1)  ,0:mx(2)-1,1)-n(1:mx(1)  ,0:mx(2)-1,2))/ln(1:mx(1)  ,0:mx(2)-1)&
    -(n(0:mx(1)-1,1:mx(2)  ,1)-n(0:mx(1)-1,1:mx(2)  ,2))/ln(0:mx(1)-1,1:mx(2)  )&
    -(n(0:mx(1)-1,0:mx(2)-1,1)+n(0:mx(1)-1,0:mx(2)-1,2))/ln(0:mx(1)-1,0:mx(2)-1)

curv = -0.5*curv/h

end function CurvatureKim2D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
function Curvature2D(c,h) result(curv)
use NodeInfoDef
use ProblemDef
implicit none
real, dimension(0:,0:), intent(in):: c
real, intent(in) :: h
!
integer, dimension(2) :: mx
real, dimension(1:size(c,1)-2,1:size(c,2)-2):: curv
real, dimension(1:size(c,1)-2,1:size(c,2)-2):: c_xx,c_yy,c_xy,c_x,c_y,denom
real :: invh,invh2
real, parameter :: small=1e-6

mx(1) = size(c,1)-2; mx(2) = size(c,2)-2

invh = 1.0/h
invh2 = invh*invh

c_xx = (c(2:mx(1)+1,1:mx(2))+c(0:mx(1)-1,1:mx(2))-2.0*c(1:mx(1),1:mx(2)))*invh2
c_yy = (c(1:mx(1),2:mx(2)+1)+c(1:mx(1),0:mx(2)-1)-2.0*c(1:mx(1),1:mx(2)))*invh2

c_x = 0.5*(c(2:mx(1)+1,1:mx(2))-c(0:mx(1)-1,1:mx(2)))*invh
c_y = 0.5*(c(1:mx(1),2:mx(2)+1)-c(1:mx(1),0:mx(2)-1))*invh

c_xy = 0.25*(c(2:mx(1)+1,2:mx(2)+1) - c(2:mx(1)+1,0:mx(2)-1) &
     +       c(0:mx(1)-1,0:mx(2)-1) - c(0:mx(1)-1,2:mx(2)+1))*invh2

denom = (c_x**2 + c_y**2)**1.5

curv = 0.0
where(abs(denom) > small)
  curv = (c_x**2*c_yy - 2.0*c_x*c_y*c_xy + c_y**2*c_xx)/denom
end where

end function Curvature2D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
! 3D Multigrid Routines:
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
subroutine RelaxGrid3D(qnew,qold,aux,f,mx,nrvars,maux,h,xlower,ipass)
use NodeInfoDef
use ProblemDef
implicit none
!
real, dimension(0:,0:,0:,1:), intent(in out):: qnew
real, dimension(0:,0:,0:,1:), intent(in):: qold
real, dimension(0:,0:,0:,1:), intent(in):: aux
real, dimension(1:,1:,1:,1:), intent(in):: f
integer, dimension(1:3), intent(in):: mx
integer, intent(in):: nrvars
integer, intent(in):: maux
real, intent(in):: h
real, dimension(1:3), intent(in):: xlower
integer, intent(in):: ipass
!
integer:: i, j, k
real:: det, dn1, dn2, dn3, dn4, dn5, dn6, h2, m1, m2, m3, m4, m5, m6, &
                omega1, omega2, tmp1, tmp2, tmp3, tmp4
real, dimension(1:2):: b
real, dimension(1:2,1:2):: a
!
qnew = 0.0
!
end subroutine RelaxGrid3D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
subroutine UpdateAuxVcycle3D(qnew,qold,aux,ll,mx,nrvars,maux,mbc,h,xlower)
use NodeInfoDef
use ProblemDef
implicit none
!
real, dimension(ll:,ll:,ll:,1:), intent(in):: qnew
real, dimension(ll:,ll:,ll:,1:), intent(in):: qold
real, dimension(ll:,ll:,ll:,1:), intent(in out):: aux
integer, intent(in):: ll
integer, dimension(1:3), intent(in):: mx
integer, intent(in):: nrvars
integer, intent(in):: maux
integer, intent(in):: mbc
real, intent(in):: h
real, dimension(1:3), intent(in):: xlower
!
!
end subroutine UpdateAuxVcycle3D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
function Operator3D(qnew,qold,aux,mx,nrvars,maux,h,xlower) result(opresult)
use NodeInfoDef
use GridUtilities, only: ULap3D
use ProblemDef
implicit none
!
real, dimension(0:,0:,0:,1:), intent(in):: qnew
real, dimension(0:,0:,0:,1:), intent(in):: qold
real, dimension(0:,0:,0:,1:), intent(in):: aux
integer, dimension(1:3), intent(in):: mx
integer, intent(in):: nrvars
integer, intent(in):: maux
real, intent(in):: h
real, dimension(1:3), intent(in):: xlower
real, dimension(1:mx(1),1:mx(2),1:mx(3),1:nrvars):: opresult
!
opresult = 0.0
!
end function Operator3D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
function Source3D(qnew,qold,aux,ll,mx,nrvars,maux,mbc,h,xlower) &
         result(sourceresult)
use NodeInfoDef
use GridUtilities, only: ULap3D
use ProblemDef
implicit none
!
real, dimension(ll:,ll:,ll:,1:), intent(in):: qnew
real, dimension(ll:,ll:,ll:,1:), intent(in):: qold
real, dimension(ll:,ll:,ll:,1:), intent(in):: aux
integer, intent(in):: ll
integer, dimension(1:3), intent(in):: mx
integer, intent(in):: nrvars
integer, intent(in):: maux
integer, intent(in):: mbc
real, intent(in):: h
real, dimension(1:3), intent(in):: xlower
real, dimension(1:mx(1),1:mx(2),1:mx(3),1:nrvars):: sourceresult
!
sourceresult = 0.0
!
end function Source3D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
function SourceUpdate3D(qnew,qold,aux,ll,mx,nrvars,maux,mbc,h,xlower) &
         result(updateresult)
use NodeInfoDef
use GridUtilities, only: UDiv3D, ULap3D
use ProblemDef
implicit none
!
real, dimension(ll:,ll:,ll:,1:), intent(in):: qnew
real, dimension(ll:,ll:,ll:,1:), intent(in):: qold
real, dimension(ll:,ll:,ll:,1:), intent(in):: aux
integer, intent(in):: ll
integer, dimension(1:3), intent(in):: mx
integer, intent(in):: nrvars
integer, intent(in):: maux
integer, intent(in):: mbc
real, intent(in):: h
real, dimension(1:3), intent(in):: xlower
real, dimension(1:mx(1),1:mx(2),1:mx(3),1:nrvars):: updateresult
!
updateresult = 0.0
!
end function SourceUpdate3D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
function ULapMob3D(c,a) result(ulapmobilresult)
use NodeInfoDef
use GridUtilities, only: UDiv3D
implicit none
!
! Level independent 3D UNDIVIDED laplacian operator for non-constant mobility.
!
real, dimension(0:,0:,0:), intent(in):: c
real, dimension(0:,0:,0:), intent(in):: a
real, dimension(1:size(a,1)-2, &
                         1:size(a,2)-2, &
                         1:size(a,3)-2):: ulapmobilresult
!
integer, dimension(1:3):: mx
real, dimension(0:size(a,1)-2  ,1:size(a,2)-2  ,1:size(a,3)-2  ):: f1
real, dimension(1:size(a,1)-2  ,0:size(a,2)-2  ,1:size(a,3)-2  ):: f2
real, dimension(1:size(a,1)-2  ,1:size(a,2)-2  ,0:size(a,3)-2  ):: f3
real, dimension(0:size(a,1)-2+1,0:size(a,2)-2+1,0:size(a,3)-2+1):: m
!
mx(1) = size(a,1)-2; mx(2) = size(a,2)-2; mx(3) = size(a,3)-2
!
m(0:mx(1)+1,0:mx(2)+1,0:mx(3)+1) &
  = 0.5*Mob(c(0:mx(1)+1,0:mx(2)+1,0:mx(3)+1))
!
! Calculate the 3D flux function:
f1(0:mx(1),1:mx(2),1:mx(3)) &
  = (m(1:mx(1)+1,1:mx(2),1:mx(3))+m(0:mx(1),1:mx(2),1:mx(3))) &
  * (a(1:mx(1)+1,1:mx(2),1:mx(3))-a(0:mx(1),1:mx(2),1:mx(3)))
!
f2(1:mx(1),0:mx(2),1:mx(3)) &
  = (m(1:mx(1),1:mx(2)+1,1:mx(3))+m(1:mx(1),0:mx(2),1:mx(3))) &
  * (a(1:mx(1),1:mx(2)+1,1:mx(3))-a(1:mx(1),0:mx(2),1:mx(3)))
!
f3(1:mx(1),1:mx(2),0:mx(3)) &
  = (m(1:mx(1),1:mx(2),1:mx(3)+1)+m(1:mx(1),1:mx(2),0:mx(3))) &
  * (a(1:mx(1),1:mx(2),1:mx(3)+1)-a(1:mx(1),1:mx(2),0:mx(3)))
!
! Calculate the divergence of the flux:
ulapmobilresult(1:mx(1),1:mx(2),1:mx(3)) &
    = UDiv3D(f1(0:mx(1),1:mx(2),1:mx(3)), &
             f2(1:mx(1),0:mx(2),1:mx(3)), &
             f3(1:mx(1),1:mx(2),0:mx(3)))
!
end function ULapMob3D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
function ULapDiff3D(c,a) result(ulapdiffresult)
use NodeInfoDef
use GridUtilities, only: UDiv3D
implicit none
!
! Level independent 3D UNDIVIDED laplacian operator for non-constant 
! diffusivity.
!
real, dimension(0:,0:,0:), intent(in):: c
real, dimension(0:,0:,0:), intent(in):: a
real, dimension(1:size(a,1)-2, &
                         1:size(a,2)-2, &
                         1:size(a,3)-2):: ulapdiffresult
!
integer, dimension(1:3):: mx
real, dimension(0:size(a,1)-2  ,1:size(a,2)-2  ,1:size(a,3)-2  ):: f1
real, dimension(1:size(a,1)-2  ,0:size(a,2)-2  ,1:size(a,3)-2  ):: f2
real, dimension(1:size(a,1)-2  ,1:size(a,2)-2  ,0:size(a,3)-2  ):: f3
real, dimension(0:size(a,1)-2+1,0:size(a,2)-2+1,0:size(a,3)-2+1):: d
!
mx(1) = size(a,1)-2; mx(2) = size(a,2)-2; mx(3) = size(a,3)-2
!
d(0:mx(1)+1,0:mx(2)+1,0:mx(3)+1) &
  = 0.5*Diff(c(0:mx(1)+1,0:mx(2)+1,0:mx(3)+1))
!
! Calculate the 3D flux function:
f1(0:mx(1),1:mx(2),1:mx(3)) &
  = (d(1:mx(1)+1,1:mx(2),1:mx(3))+d(0:mx(1),1:mx(2),1:mx(3))) &
  * (a(1:mx(1)+1,1:mx(2),1:mx(3))-a(0:mx(1),1:mx(2),1:mx(3)))
!
f2(1:mx(1),0:mx(2),1:mx(3)) &
  = (d(1:mx(1),1:mx(2)+1,1:mx(3))+d(1:mx(1),0:mx(2),1:mx(3))) &
  * (a(1:mx(1),1:mx(2)+1,1:mx(3))-a(1:mx(1),0:mx(2),1:mx(3)))
!
f3(1:mx(1),1:mx(2),0:mx(3)) &
  = (d(1:mx(1),1:mx(2),1:mx(3)+1)+d(1:mx(1),1:mx(2),0:mx(3))) &
  * (a(1:mx(1),1:mx(2),1:mx(3)+1)-a(1:mx(1),1:mx(2),0:mx(3)))
!
! Calculate the divergence of the flux:
ulapdiffresult(1:mx(1),1:mx(2),1:mx(3)) &
   = UDiv3D(f1(0:mx(1),1:mx(2),1:mx(3)), &
            f2(1:mx(1),0:mx(2),1:mx(3)), &
            f3(1:mx(1),1:mx(2),0:mx(3)))
!
end function ULapDiff3D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
function UEno3D(c,u1,u2,u3) result(uenoresult)
use NodeInfoDef
implicit none
!
real, dimension(-1:,-1:,-1:), intent(in):: c
real, dimension(0:,1:,1:), intent(in):: u1
real, dimension(1:,0:,1:), intent(in):: u2
real, dimension(1:,1:,0:), intent(in):: u3
real, dimension(1:size(c,1)-4, &
                         1:size(c,2)-4, &
                         1:size(c,3)-4):: uenoresult
!
integer, dimension(1:3):: mx
real, dimension(0:size(c,1)-4, &
                         1:size(c,2)-4, &
                         1:size(c,3)-4):: f1
real, dimension(1:size(c,1)-4, &
                         0:size(c,2)-4, &
                         1:size(c,3)-4):: f2
real, dimension(1:size(c,1)-4, &
                         1:size(c,2)-4, &
                         0:size(c,3)-4):: f3
!
integer:: i, i1, j, j1, k, k1
real:: aa, bb, cc
!
mx(1) = size(c,1)-4; mx(2) = size(c,2)-4; mx(3) = size(c,3)-4
!
do i = 0, mx(1)
  do j = 1, mx(2)
    do k = 1, mx(3)
      if(u1(i,j,k)>0.0) then
        i1 = i
      else
        i1 = i+1
      end if
      aa = c(i1,j,k)-c(i1-1,j,k)
      bb = c(i1+1,j,k)-c(i1,j,k)
      if(abs(aa)<=abs(bb)) then
        cc = aa
      else
        cc = bb
      end if
      f1(i,j,k) = (c(i1,j,k)+cc*(1.0-2.0*real(i1-i,kind=r8))/2.0) &
                * u1(i,j,k)
    end do
  end do
end do
!
do i = 1, mx(1)
  do j = 0, mx(2)
    do k = 1, mx(3)
      if(u2(i,j,k)>0.0) then
        j1 = j
      else
        j1 = j+1
      end if
      aa = c(i,j1,k)-c(i,j1-1,k)
      bb = c(i,j1+1,k)-c(i,j1,k)
      if(abs(aa)<=abs(bb)) then
        cc = aa
      else
        cc = bb
      end if
      f2(i,j,k) = (c(i,j1,k)+cc*(1.0-2.0*real(j1-j,kind=r8))/2.0) &
                * u2(i,j,k)
    end do
  end do
end do
!
do i = 1, mx(1)
  do j = 1, mx(2)
    do k = 0, mx(3)
      if(u3(i,j,k)>0.0) then
        k1 = k
      else
        k1 = k+1
      end if
      aa = c(i,j,k1)-c(i,j,k1-1)
      bb = c(i,j,k1+1)-c(i,j,k1)
      if(abs(aa)<=abs(bb)) then
        cc = aa
      else
        cc = bb
      end if
      f3(i,j,k) = (c(i,j,k1)+cc*(1.0-2.0*real(k1-k,kind=r8))/2.0) &
                * u3(i,j,k)
    end do
  end do
end do
!
uenoresult(1:mx(1),1:mx(2),1:mx(3)) &
  =  f1(1:mx(1),1:mx(2),1:mx(3))-f1(0:mx(1)-1,1:mx(2)  ,1:mx(3)  ) &
  +  f2(1:mx(1),1:mx(2),1:mx(3))-f2(1:mx(1)  ,0:mx(2)-1,1:mx(3)  ) &
  +  f3(1:mx(1),1:mx(2),1:mx(3))-f3(1:mx(1)  ,1:mx(2)  ,0:mx(3)-1)
!
end function UEno3D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
function UWeno3D(c,u,v,w) result(uwenoresult)
use NodeInfoDef
implicit none
!
! 3rd-order WENO reconstruction.
!
real, dimension(-1:,-1:,-1:), intent(in):: c
real, dimension(0:,1:,1:):: u
real, dimension(1:,0:,1:):: v
real, dimension(1:,1:,0:):: w
real, dimension(1:size(c,1)-4, &
                         1:size(c,2)-4, &
                         1:size(c,3)-4):: uwenoresult
!
integer:: i, j, k
integer, dimension(1:3):: mx
real:: a11, a12, a21, a22, b1, b2, &
                g1, g2, v1, v2, w1, w2, f1, f2
real, dimension(0:size(c,1)-4, &
                         1:size(c,2)-4, &
                         1:size(c,3)-4):: few
real, dimension(1:size(c,1)-4, &
                         0:size(c,2)-4, &
                         1:size(c,3)-4):: fns
real, dimension(1:size(c,1)-4, &
                         1:size(c,2)-4, &
                         0:size(c,3)-4):: fio
!
a11 = -01.0/2.0; a12 = 03.0/2.0
a21 =  01.0/2.0; a22 = 01.0/2.0
!
g1 = 01.0/03.0
g2 = 02.0/03.0
!
mx(1) = size(c,1)-4; mx(2) = size(c,2)-4; mx(3) = size(c,3)-4
!
do i = 0, mx(1)
  do j = 1, mx(2)
    do k = 1, mx(3)
      if(u(i,j,k)>0.0) then
        f1 = a11*c(i-1,j,k)+a12*c(i  ,j,k)
        f2 = a21*c(i  ,j,k)+a22*c(i+1,j,k)
        b1 = Sqr(c(i  ,j,k)-    c(i-1,j,k))
        b2 = Sqr(c(i+1,j,k)-    c(i  ,j,k))
        v1 = g1/Sqr(1.0E-06+b1); v2 = g2/Sqr(1.0E-06+b2)
        w1 = v1/(v1+v2); w2 = v2/(v1+v2)
      else
        f1 = a11*c(i+2,j,k)+a12*c(i+1,j,k)
        f2 = a21*c(i+1,j,k)+a22*c(i  ,j,k)
        b1 = Sqr(c(i+2,j,k)-    c(i+1,j,k))
        b2 = Sqr(c(i+1,j,k)-    c(i  ,j,k))
        v1 = g1/Sqr(1.0E-06+b1); v2 = g2/Sqr(1.0E-06+b2)
        w1 = v1/(v1+v2); w2 = v2/(v1+v2)
      end if
      few(i,j,k) = (f1*w1+f2*w2)*u(i,j,k)
    end do
  end do
end do
!
do i = 1, mx(1)
  do j = 0, mx(2)
    do k = 1, mx(3)
      if(v(i,j,k)>0.0) then
        f1 = a11*c(i,j-1,k)+a12*c(i,j  ,k)
        f2 = a21*c(i,j  ,k)+a22*c(i,j+1,k)
        b1 = Sqr(c(i,j  ,k)-    c(i,j-1,k))
        b2 = Sqr(c(i,j+1,k)-    c(i,j  ,k))
        v1 = g1/Sqr(1.0E-06+b1); v2 = g2/Sqr(1.0E-06+b2)
        w1 = v1/(v1+v2); w2 = v2/(v1+v2)
      else
        f1 = a11*c(i,j+2,k)+a12*c(i,j+1,k)
        f2 = a21*c(i,j+1,k)+a22*c(i,j  ,k)
        b1 = Sqr(c(i,j+2,k)-    c(i,j+1,k))
        b2 = Sqr(c(i,j+1,k)-    c(i,j  ,k))
        v1 = g1/Sqr(1.0E-06+b1); v2 = g2/Sqr(1.0E-06+b2)
        w1 = v1/(v1+v2); w2 = v2/(v1+v2)
      end if
      fns(i,j,k) = (f1*w1+f2*w2)*v(i,j,k)
    end do
  end do
end do
!
do i = 1, mx(1)
  do j = 1, mx(2)
    do k = 0, mx(3)
      if(w(i,j,k)>0.0) then
        f1 = a11*c(i,j,k-1)+a12*c(i,j,k  )
        f2 = a21*c(i,j,k  )+a22*c(i,j,k+1)
        b1 = Sqr(c(i,j,k  )-    c(i,j,k-1))
        b2 = Sqr(c(i,j,k+1)-    c(i,j,k  ))
        v1 = g1/Sqr(1.0E-06+b1); v2 = g2/Sqr(1.0E-06+b2)
        w1 = v1/(v1+v2); w2 = v2/(v1+v2)
      else
        f1 = a11*c(i,j,k+2)+a12*c(i,j,k+1)
        f2 = a21*c(i,j,k+1)+a22*c(i,j,k  )
        b1 = Sqr(c(i,j,k+2)-    c(i,j,k+1))
        b2 = Sqr(c(i,j,k+1)-    c(i,j,k  ))
        v1 = g1/Sqr(1.0E-06+b1); v2 = g2/Sqr(1.0E-06+b2)
        w1 = v1/(v1+v2); w2 = v2/(v1+v2)
      end if
      fio(i,j,k) = (f1*w1+f2*w2)*w(i,j,k)
    end do
  end do
end do
!
uwenoresult(1:mx(1),1:mx(2),1:mx(3)) &
  =  few(1:mx(1),1:mx(2),1:mx(3))-few(0:mx(1)-1,1:mx(2)  ,1:mx(3)  ) &
  +  fns(1:mx(1),1:mx(2),1:mx(3))-fns(1:mx(1)  ,0:mx(2)-1,1:mx(3)  ) &
  +  fio(1:mx(1),1:mx(2),1:mx(3))-fio(1:mx(1)  ,1:mx(2)  ,0:mx(3)-1)
!!!!
end function UWeno3D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
! User supplied physical boundary conditions:
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
subroutine UserBC2D(q,ll,mx,nrvars,h,xlower,mbc,bcnow)
use NodeinfoDef
use ProblemDef
implicit none
!
real, dimension(ll:,ll:,1:), intent(in out):: q
integer, intent(in):: ll
integer, dimension(1:2), intent(in):: mx
integer, intent(in):: nrvars
real, intent(in):: h
real, dimension(1:2), intent(in):: xlower
integer, intent(in):: mbc
integer, intent(in):: bcnow
!
integer:: ibc
integer, dimension(1:2):: ul
real, dimension(2) :: xmax
integer :: i,j
real :: x,y
real :: cnst = 1.0
real, parameter :: vel = 0.5

if(axisym) cnst=1.0
!
ul(1:2) = mx(1:2)+mbc

xmax = xlower + mx*h
!
! qu : No-slip
! qv : No-slip
! qp : Extrapolation
! qpsi : Neumann
! TODO rest of vars
select case(bcnow)
  case(1) ! West
    do ibc = 1, mbc
      if (axisym) then
        q(1-ibc,ll:ul(2),:) = q(ibc,ll:ul(2),:) 
        q(1-ibc,ll:ul(2),qus) = -q(ibc,ll:ul(2),qus) 
        q(1-ibc,ll:ul(2),qu)  = -q(ibc,ll:ul(2),qu) 
        q(1-ibc,ll:ul(2),qum) = -q(ibc,ll:ul(2),qum) 
      else
        ! Specified velocities
!       x = (real(mx(1)+ibc)-0.5)*h+xlower(1)
      do j=-1,mx(2)+2
        y = (real(j)-0.5)*h+xlower(2)
        q(1-ibc,j,qus) = 2.0*cavity_speed*y/xmax(2)- q(ibc,j,qus) 
        q(1-ibc,j,qu)  = 2.0*cavity_speed*y/xmax(2)- q(ibc,j,qu)
        q(1-ibc,j,qum) = 2.0*cavity_speed*y/xmax(2)- q(ibc,j,qum)
        q(1-ibc,j,qvs) = -q(ibc,j,qvs)
        q(1-ibc,j,qv)  = -q(ibc,j,qv)
        q(1-ibc,j,qvm) = -q(ibc,j,qvm)
      enddo
        ! Symmetry
!         q(1-ibc,ll:ul(2),:) = q(ibc,ll:ul(2),:) 
!         q(1-ibc,ll:ul(2),qus) = -q(ibc,ll:ul(2),qus) 
!         q(1-ibc,ll:ul(2),qu)  = -q(ibc,ll:ul(2),qu) 
!         q(1-ibc,ll:ul(2),qum) = -q(ibc,ll:ul(2),qum) 
        ! "Normal"
!         q(1-ibc,ll:ul(2),qus:qvm) =-q(ibc,ll:ul(2),qus:qvm) 
        q(1-ibc,ll:ul(2),qpsi) = q(ibc,ll:ul(2),qpsi)
        q(1-ibc,ll:ul(2),qmac) = q(ibc,ll:ul(2),qmac)
        q(1-ibc,ll:ul(2),qp) = 2.0*q(2-ibc,ll:ul(2),qp)-q(3-ibc,ll:ul(2),qp)
        q(1-ibc,ll:ul(2),qc ) = q(ibc,ll:ul(2),qc ) 
        q(1-ibc,ll:ul(2),qmu) = q(ibc,ll:ul(2),qmu)
        q(1-ibc,ll:ul(2),qb) = 2.0*b_eq - q(ibc,ll:ul(2),qb)
        q(1-ibc,ll:ul(2),qf) = 2.0*f_eq - q(ibc,ll:ul(2),qf)
!         q(1-ibc,ll:ul(2),qf2) = 2.0*f_eq -q(ibc,ll:ul(2),qf2)

      endif
    end do
  case(2) ! East
    do ibc = 1, mbc
      ! Specified velocities
!       x = (real(mx(1)+ibc)-0.5)*h+xlower(1)
      do j=-1,mx(2)+2
        y = (real(j)-0.5)*h+xlower(2)
        q(mx(1)+ibc,j,qus) =2.0*cavity_speed*y/xmax(2)- q(mx(1)-ibc+1,j,qus) 
        q(mx(1)+ibc,j,qu)  =2.0*cavity_speed*y/xmax(2)- q(mx(1)-ibc+1,j,qu)
        q(mx(1)+ibc,j,qum) =2.0*cavity_speed*y/xmax(2)- q(mx(1)-ibc+1,j,qum)
        q(mx(1)+ibc,j,qvs) =-q(mx(1)-ibc+1,j,qvs)
        q(mx(1)+ibc,j,qv)  =-q(mx(1)-ibc+1,j,qv)
        q(mx(1)+ibc,j,qvm) =-q(mx(1)-ibc+1,j,qvm)
      enddo
      ! Symmetry
!       q(mx(1)+ibc,ll:ul(2),:) = q(mx(1)-ibc+1,ll:ul(2),:)
!       q(mx(1)+ibc,ll:ul(2),qus) = -q(mx(1)-ibc+1,ll:ul(2),qus)
!       q(mx(1)+ibc,ll:ul(2),qu)  = -q(mx(1)-ibc+1,ll:ul(2),qu)
!       q(mx(1)+ibc,ll:ul(2),qum) = -q(mx(1)-ibc+1,ll:ul(2),qum)
      ! "Normal"
!       q(mx(1)+ibc,ll:ul(2),qus:qvm) =-q(mx(1)-ibc+1,ll:ul(2),qus:qvm)
      q(mx(1)+ibc,ll:ul(2),qpsi) = q(mx(1)-ibc+1,ll:ul(2),qpsi)
      q(mx(1)+ibc,ll:ul(2),qmac) = q(mx(1)-ibc+1,ll:ul(2),qmac)
      q(mx(1)+ibc,ll:ul(2),qp) = 2.0*q(mx(1)+ibc-1,ll:ul(2),qp) &
                               -     q(mx(1)+ibc-2,ll:ul(2),qp)
      q(mx(1)+ibc,ll:ul(2),qc ) = q(mx(1)-ibc+1,ll:ul(2),qc)
      q(mx(1)+ibc,ll:ul(2),qmu) = q(mx(1)-ibc+1,ll:ul(2),qmu)
      q(mx(1)+ibc,ll:ul(2),qb) = 2.0*b_eq - q(mx(1)-ibc+1,ll:ul(2),qb)
      q(mx(1)+ibc,ll:ul(2),qf) = 2.0*f_eq - q(mx(1)-ibc+1,ll:ul(2),qf)
!       q(mx(1)+ibc,ll:ul(2),qf2) = 2.0*f_eq-q(mx(1)-ibc+1,ll:ul(2),qf2)
    end do
  case(3) ! South
    do ibc = 1, mbc
      ! Specified velocity
!       y = (real(0.0+ibc)-0.5)*h+xlower(2)
!       do i=-1,mx(1)+2
!         x = (real(i)-0.5)*h+xlower(1)
!         q(i,1-ibc,qus) =- q(i,ibc,qus)
!         q(i,1-ibc,qu)  =- q(i,ibc,qu)
!         q(i,1-ibc,qum) =- q(i,ibc,qum)
!         q(i,1-ibc,qvs) = 2.0*vel*(1.0-0.0*abs(x)) - q(i,ibc,qvs)
!         q(i,1-ibc,qv)  = 2.0*vel*(1.0-0.0*abs(x)) - q(i,ibc,qv)
!         q(i,1-ibc,qvm) = 2.0*vel*(1.0-0.0*abs(x)) - q(i,ibc,qvm)
!       enddo
      ! Anti-symmetry in velocities
!       q(ll:ul(1),1-ibc,:)   =  q(ll:ul(1),ibc,:)
!       q(ll:ul(1),1-ibc,qus) = -q(ll:ul(1),ibc,qus)
!       q(ll:ul(1),1-ibc,qu ) = -q(ll:ul(1),ibc,qu )
!       q(ll:ul(1),1-ibc,qum) = -q(ll:ul(1),ibc,qum)
!       q(ll:ul(1),1-ibc,qvs) = -q(ll:ul(1),ibc,qvs)
!       q(ll:ul(1),1-ibc,qv ) = -q(ll:ul(1),ibc,qv )
!       q(ll:ul(1),1-ibc,qvm) = -q(ll:ul(1),ibc,qvm)
      ! Symmetry
!       q(ll:ul(1),1-ibc,:) = q(ll:ul(1),ibc,:)
!       q(ll:ul(1),1-ibc,qvs) =-q(ll:ul(1),ibc,qvs)
!       q(ll:ul(1),1-ibc,qv) =-q(ll:ul(1),ibc,qv)
!       q(ll:ul(1),1-ibc,qvm) =-q(ll:ul(1),ibc,qvm)
      ! "Normal"
      q(ll:ul(1),1-ibc,qus:qvm) =-q(ll:ul(1),ibc,qus:qvm)
      q(ll:ul(1),1-ibc,qus) =-2.0*cavity_speed-q(ll:ul(1),ibc,qus)
      q(ll:ul(1),1-ibc,qu)  =-2.0*cavity_speed-q(ll:ul(1),ibc,qu)
      q(ll:ul(1),1-ibc,qum) =-2.0*cavity_speed-q(ll:ul(1),ibc,qum)
      q(ll:ul(1),1-ibc,qpsi) = q(ll:ul(1),ibc,qpsi)
      q(ll:ul(1),1-ibc,qmac) = q(ll:ul(1),ibc,qmac)
      q(ll:ul(1),1-ibc,qp) = 2.0*q(ll:ul(1),2-ibc,qp)-q(ll:ul(1),3-ibc,qp)
      q(ll:ul(1),1-ibc,qc ) = q(ll:ul(1),ibc,qc)
      q(ll:ul(1),1-ibc,qmu) = q(ll:ul(1),ibc,qmu)
      q(ll:ul(1),1-ibc,qb) = 2.0*b_eq - q(ll:ul(1),ibc,qb)
      q(ll:ul(1),1-ibc,qf) = 2.0*f_eq - q(ll:ul(1),ibc,qf)
!       q(ll:ul(1),1-ibc,qf2) = 2.0*f_eq-q(ll:ul(1),ibc,qf2)
      q(ll:ul(1),1-ibc,qp) = q(ll:ul(1),ibc,qp)
      q(ll:ul(1),1-ibc,qsig) = q(ll:ul(1),ibc,qsig)
    end do
  case(4) ! North
    do ibc = 1, mbc
!       y = (real(mx(2)+ibc)-0.5)*h+xlower(2)
!       do i=-1,mx(1)+2
!         x = (real(i)-0.5)*h+xlower(1)
!         q(i,mx(2)+ibc,qus) =-0.0*x- q(i,mx(2)-ibc+1,qus)
!         q(i,mx(2)+ibc,qu)  =-0.0*x- q(i,mx(2)-ibc+1,qu)
!         q(i,mx(2)+ibc,qum) =-0.0*x- q(i,mx(2)-ibc+1,qum)
!         q(i,mx(2)+ibc,qvs) = cnst*vel*2.0*(1.0-0.0*x) -  q(i,mx(2)-ibc+1,qvs)
!         q(i,mx(2)+ibc,qv)  = cnst*vel*2.0*(1.0-0.0*x) -  q(i,mx(2)-ibc+1,qv)
!         q(i,mx(2)+ibc,qvm) = cnst*vel*2.0*(1.0-0.0*x) -  q(i,mx(2)-ibc+1,qvm)
!       enddo
      ! "Normal"
      q(ll:ul(1),mx(2)+ibc,qus:qvm)= -q(ll:ul(1),mx(2)-ibc+1,qus:qvm)
      q(ll:ul(1),mx(2)+ibc,qus)= 2.0*cavity_speed-q(ll:ul(1),mx(2)-ibc+1,qus)
      q(ll:ul(1),mx(2)+ibc,qu )= 2.0*cavity_speed-q(ll:ul(1),mx(2)-ibc+1,qu )
      q(ll:ul(1),mx(2)+ibc,qum)= 2.0*cavity_speed-q(ll:ul(1),mx(2)-ibc+1,qum)
      q(ll:ul(1),mx(2)+ibc,qpsi) = q(ll:ul(1),mx(2)-ibc+1,qpsi)
      q(ll:ul(1),mx(2)+ibc,qmac) = q(ll:ul(1),mx(2)-ibc+1,qmac)
      q(ll:ul(1),mx(2)+ibc,qp) = 2.0*q(ll:ul(1),mx(2)+ibc-1,qp) &
                               -     q(ll:ul(1),mx(2)+ibc-2,qp)
      q(ll:ul(1),mx(2)+ibc,qc ) = q(ll:ul(1),mx(2)-ibc+1,qc)
      q(ll:ul(1),mx(2)+ibc,qmu) = q(ll:ul(1),mx(2)-ibc+1,qmu)
      q(ll:ul(1),mx(2)+ibc,qb) = 2.0*b_eq - q(ll:ul(1),mx(2)-ibc+1,qb)
      q(ll:ul(1),mx(2)+ibc,qf) = 2.0*f_eq -  q(ll:ul(1),mx(2)-ibc+1,qf)
!       q(ll:ul(1),mx(2)+ibc,qf2) = 2.0*f_eq-q(ll:ul(1),mx(2)-ibc+1,qf2)
      q(ll:ul(1),mx(2)+ibc,qsig) = q(ll:ul(1),mx(2)-ibc+1,qsig)
    end do
end select
!
end subroutine UserBC2D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
subroutine UserBC3D(q,ll,mx,nrvars,mbc,bcnow)
use NodeinfoDef
implicit none
!
real, dimension(ll:,ll:,ll:,1:), intent(in out):: q
integer, intent(in):: ll
integer, dimension(1:3), intent(in):: mx
integer, intent(in):: nrvars
integer, intent(in):: mbc
integer, intent(in):: bcnow
!
integer:: ibc
integer, dimension(1:3):: ul
!
ul(1:3) = mx(1:3)+mbc
!
select case(bcnow)
  case(1)
!
! Homogeneous Neumann on bndry 1:
    do ibc = 1, mbc
      q(1-ibc,ll:ul(2),ll:ul(3),1) = q(ibc,ll:ul(2),ll:ul(3),1)
    end do
!
! Dirichlet on bndry 1:
    do ibc = 1, mbc
      q(1-ibc,ll:ul(2),ll:ul(3),2) =       -q(ibc,ll:ul(2),ll:ul(3),2)
    end do
!
  case(2)
!
! Homogeneous Neumann on bndry 2:
    do ibc = 1, mbc
      q(mx(1)+ibc,ll:ul(2),ll:ul(3),1) = q(mx(1)-ibc+1,ll:ul(2),ll:ul(3),1)
    end do
!
! Dirichlet on bndry 2:
    do ibc = 1, mbc
      q(mx(1)+ibc,ll:ul(2),ll:ul(3),2) =       -q(mx(1)-ibc+1,ll:ul(2),ll:ul(3),2)
    end do
!
  case(3)
!
! Homogeneous Neumann on bndry 3:
    do ibc = 1, mbc
      q(ll:ul(1),1-ibc,ll:ul(3),1) = q(ll:ul(1),ibc,ll:ul(3),1)
    end do
!
! Dirichlet on bndry 3:
    do ibc = 1, mbc
      q(ll:ul(1),1-ibc,ll:ul(3),2) =       -q(ll:ul(1),ibc,ll:ul(3),2)
    end do
!
  case(4)
!
! Homogeneous Neumann on bndry 4:
    do ibc = 1, mbc
      q(ll:ul(1),mx(2)+ibc,ll:ul(3),1) = q(ll:ul(1),mx(2)-ibc+1,:,1)
    end do
!
! Dirichlet on bndry 4:
    do ibc = 1, mbc
      q(ll:ul(1),mx(2)+ibc,ll:ul(3),2) =       -q(ll:ul(1),mx(2)-ibc+1,ll:ul(3),2)
    end do
!
  case(5)
!
! Homogeneous Neumann on bndry 5:
    do ibc = 1, mbc
      q(ll:ul(1),ll:ul(2),1-ibc,1) = q(ll:ul(1),ll:ul(2),ibc,1)
    end do
!
! Dirichlet on bndry 5:
    do ibc = 1, mbc
      q(ll:ul(1),ll:ul(2),1-ibc,2) =       -q(ll:ul(1),ll:ul(2),ibc,2)
    end do
!
  case(6)
!
! Homogeneous Neumann on bndry 6:
    do ibc = 1, mbc
      q(ll:ul(1),ll:ul(2),mx(3)+ibc,1) = q(ll:ul(1),ll:ul(2),mx(3)-ibc+1,1)
    end do
!
! Dirichlet on bndry 6:
    do ibc = 1, mbc
      q(ll:ul(1),ll:ul(2),mx(3)+ibc,2) =       -q(ll:ul(1),ll:ul(2),mx(3)-ibc+1,2)
    end do
end select
!
end subroutine UserBC3D
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
! Dimensionally Invariant Multigrid Routines:
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
elemental function Delta(c) result(deltaresult)
use NodeInfoDef
use ProblemDef
implicit none
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
real, intent(in):: c
real:: deltaresult,mc
real, parameter :: small = 1e-6
real :: const  

const = 3.0*sqrt(2.0)/eps

! Should be multiplied by alpha, but this gets cancelled out in
! the surfactant equation
mc = 1.0-c
deltaresult = const*c*c*mc*mc + small

end function Delta
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
elemental function Smear(a1,a2,c) result(smeared)
! Smear a property according to the local concentration
use NodeInfoDef
use ProblemDef
implicit none
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
real, intent(in):: a1,a2
real, intent(in):: c
real :: smeared

smeared = a1*c + a2*(1.0-c)

end function Smear
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
elemental function SmearHarm(a1,a2,c) result(smeared)
! Smear a property harmonically according to the local concentration
use NodeInfoDef
use ProblemDef
implicit none
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
real, intent(in):: a1,a2
real, intent(in):: c
real :: smeared

smeared = a1*a2/(a2*c + a1*(1.0-c))

end function SmearHarm
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

pure function Delta2(c) result(delresult)
! Undivided |grad(c)|
use NodeInfoDef
use ProblemDef
implicit none
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
real, dimension(3,3), intent(in):: c

real:: delresult
real, parameter :: small = 1e-6
real :: invh,c_x,c_y

c_x = c(3,2) - c(1,2)
c_y = c(2,3) - c(2,1)

delresult = 0.5*sqrt(c_x*c_x + c_y*c_y) + small

end function Delta2
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
elemental function Heavi(c) result(heaviresult)
use NodeInfoDef
use ProblemDef
implicit none
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
real, intent(in):: c
real:: heaviresult
real,parameter:: small=1e-6

heaviresult = abs(1-c) + small

end function Heavi

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
elemental function Mob(c) result(mobilresult)
use NodeInfoDef
use ProblemDef
implicit none
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
real, intent(in):: c
real:: mobilresult,mc
real, parameter :: small = 1e-6

! This definition ensures that M stays positive
! mobilresult = sqrt(eps*c*c*(1.0-c)**2 + small)
mc = 1.0-c
mobilresult = sqrt(c*c*mc*mc) + small

end function Mob
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
elemental function Diff(c) result(diffresult)
use NodeInfoDef
use ProblemDef
implicit none
!
! Non-constant diffusivity:
!
real, intent(in):: c
real:: diffresult
!
!
!
diffresult = (1.0-c)+c
!
!tmp = tmp*tmp
!tmp = tmp*tmp
!tmp = tmp*tmp ! (1-c)**8
!
!if(c<0.0) then
!  diffresult = diffnh
!else if(c>1.0) then
!  diffresult = diffnt
!else
!  diffresult = (diffnh-diffnt)*tmp+diffnt
!end if
!
end function Diff
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
elemental function Df(c) result(dfresult)
! Derivative of f-function in equation for chemical potential
! f=0.25*c^2*(1-c)^2
! f' = c*(c-1)*(c-1/2) 
use NodeInfoDef
use ProblemDef
implicit none
!
real, intent(in):: c
real:: dfresult
!
dfresult = c*(c-1.0)*(c-0.5)
!
end function Df
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
elemental function D2f(c) result(d2fresult)
! Second derivative of f-function in equation for chemical potential
! f=0.25*c^2*(1-c)^2
! f'' = 0.5 + 3*c*(c-1)
use NodeInfoDef
use ProblemDef
implicit none
!
real, intent(in):: c
real:: d2fresult
!
d2fresult = 0.5 + 3*c*(c-1)
!
end function D2f
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
elemental function Sqr(c) result(sqrresult)
use NodeInfoDef
implicit none
!
real, intent(in):: c
real:: sqrresult
!
sqrresult = c*c
!
end function Sqr

subroutine vortex(u1,u2,h,mx,xlower)
use ProblemDef
use NodeInfoDef
implicit none
real, dimension(0:,1:), intent(out) :: u1
real, dimension(1:,0:), intent(out) :: u2
real, intent(in) :: h
integer, dimension(2), intent(in) :: mx
real, dimension(2), intent(in) :: xlower
real:: tmp
real :: x,y,pi,xc,yc,freq
integer :: i,j
real, parameter :: period=1.0
real:: delta, r1, rad, theta, xcold

! Compute velocities
! vortex
pi = 4.0*atan(1.0)
xc = 0.0
yc = 0.5
freq = cos(pi*currenttime/period)
do j=1,mx(2)
  y = (real(j)-0.5)*h+xlower(2)
  do i=0,mx(1)
    x = real(i)*h+xlower(1)
    u1(i,j)= 2.0*freq*sin(pi*(x-xc))**2*cos(pi*(y-yc))*sin(pi*(y-yc))
  enddo
enddo

do j=0,mx(2)
  y = real(j)*h+xlower(2)
  do i=1,mx(1)
    x = (real(i)-0.5)*h+xlower(1)
    u2(i,j)= 2.0*freq*cos(pi*(y-yc))**2*sin(pi*(x-xc))*cos(pi*(x-xc))
  enddo
enddo

end subroutine vortex

subroutine expanding(u1,u2,h,mx,xlower)
use ProblemDef
use NodeInfoDef
implicit none
real, dimension(0:,1:), intent(out) :: u1
real, dimension(1:,0:), intent(out) :: u2
real, intent(in) :: h
integer, dimension(2), intent(in) :: mx
real, dimension(2), intent(in) :: xlower
real :: x,y,xc,yc,theta
integer :: i,j

xc = 0.0
yc = 0.0
do i = 0, mx(1)
  x = (real(i)-0.5)*h+xlower(1)
  do j = 0, mx(2)
    y = (real(j)-0.5)*h+xlower(2)
    if(j/=0) then
      theta = atan2(y-yc,x+0.5*h-xc)
      u1(i,j) = cos(theta)
    endif
    if(i/=0) then
      theta = atan2(y+0.5*h-yc,x-xc)
      u2(i,j) = sin(theta)
    endif
  end do
end do

end subroutine expanding

elemental function Normalize2D(a,lower,upper) result(norm_a)
implicit none
real, intent(in) :: a
real, intent(in) :: lower,upper
real :: norm_a

norm_a = (a-lower)/(upper-lower)

end function Normalize2D

end module Problem












